"""ROS2 Joy bridge for OmniDaemon or direct OmniSocket control.""" from __future__ import annotations import os from pathlib import Path import sys import time from typing import Dict import rclpy from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import Joy import yaml try: from .omnisocket_control import make_control_packet except ImportError: # pragma: no cover - direct script execution fallback from omnisocket_control import make_control_packet WORKSPACE_ROOT = Path(__file__).resolve().parents[3] DEFAULT_BACKEND = "daemon" def _load_daemon_client_api(): try: from omnisocket_a_side.client import OmniDaemonClient except ImportError: python_dir = WORKSPACE_ROOT / "OmniSocketGo" / "python" if python_dir.exists(): sys.path.insert(0, str(python_dir)) from omnisocket_a_side.client import OmniDaemonClient return OmniDaemonClient def _load_omnisocket_api(): try: from omnisocket import CONTROL_DEFAULTS, Session except ImportError as exc: # pragma: no cover - environment dependent raise RuntimeError( "omnisocket is not installed. Install it before using direct transport mode." ) from exc return CONTROL_DEFAULTS, Session class OmniSocketXboxSender(Node): """Subscribe to Joy messages and forward them through the selected backend.""" def __init__(self) -> None: super().__init__("omnisocket_xbox_sender") self.config: Dict[str, object] = {} self.backend = self._resolve_backend() self.seq_id = 0 self.last_buttons: Dict[str, int] = {} self.last_dpad_h = 0.0 self.session = None self.daemon_client = None self._last_transport_error = "" self._last_transport_error_at = 0.0 self._load_config() self._init_transport() qos_profile = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=10, ) self.subscription = self.create_subscription( Joy, self.joy_topic, self._joy_callback, qos_profile ) self.get_logger().info( f"Forwarding {self.joy_topic} via {self.backend} " f"from {self.peer_id} to {self.target_peer}" ) self.get_logger().info("Buttons: A=WALKAMP X=ZERO Y=STOP START=reset") @staticmethod def _resolve_backend() -> str: backend = os.getenv("OMNI_TRANSPORT_BACKEND", DEFAULT_BACKEND).strip().lower() if backend not in {"daemon", "direct"}: return DEFAULT_BACKEND return backend def destroy_node(self) -> bool: if self.session is not None: self.session.close() self.session = None if self.daemon_client is not None: self.daemon_client.close() self.daemon_client = None return super().destroy_node() def _load_config(self) -> None: omni_config_path = ( Path(__file__).resolve().parent / "config" / "omnisocket_demo.yaml" ) main_config_path = Path(__file__).resolve().parents[1] / "config" / "dex_config.yaml" if omni_config_path.exists(): with omni_config_path.open("r", encoding="utf-8") as file: self.config = yaml.safe_load(file) or {} else: self.config = {} with main_config_path.open("r", encoding="utf-8") as file: main_config = yaml.safe_load(file) or {} transport_cfg = self.config.get("transport", {}) sender_cfg = self.config.get("control_sender", {}) xbox_cfg = main_config.get("xbox", {}) self.server_addr = str(transport_cfg.get("server_addr", "127.0.0.1:10909")) self.relay_via = str(transport_cfg.get("relay_via", "")) self.bind_ip = str(transport_cfg.get("bind_ip", "")) self.bind_device = str(transport_cfg.get("bind_device", "")) self.peer_id = str(sender_cfg.get("peer_id", "peer-a-ctrl")) self.target_peer = str(sender_cfg.get("target_peer", "peer-b-ctrl")) self.joy_topic = str(sender_cfg.get("joy_topic", "/xbox_data")) self.deadzone = float(sender_cfg.get("deadzone", 0.10)) self.analog_epsilon = float(sender_cfg.get("analog_epsilon", 0.01)) self.dpad_threshold = float(sender_cfg.get("dpad_threshold", 0.50)) self.trigger_pressed_threshold = float( sender_cfg.get("trigger_pressed_threshold", -0.50) ) self.forward_command_offset = float( sender_cfg.get( "forward_command_offset", xbox_cfg.get("forward_command_offset", 0.0), ) ) self.lateral_command_offset = float( sender_cfg.get( "lateral_command_offset", xbox_cfg.get("lateral_command_offset", 0.0), ) ) self.rotation_command_offset = float( sender_cfg.get( "rotation_command_offset", xbox_cfg.get("rotation_command_offset", 0.0), ) ) self.button_map = { "a": 0, "b": 1, "x": 2, "y": 3, "lb": 4, "rb": 5, "select": 6, "start": 7, "home": 8, } self.axis_map = { "lx": 0, "ly": 1, "l_trigger": 2, "rx": 3, "ry": 4, "r_trigger": 5, "dpad_h": 6, "dpad_v": 7, } self._merge_mapping(self.button_map, xbox_cfg.get("button_map")) self._merge_mapping(self.axis_map, xbox_cfg.get("axis_map")) self._merge_mapping(self.button_map, sender_cfg.get("button_map")) self._merge_mapping(self.axis_map, sender_cfg.get("axis_map")) def _merge_mapping(self, target: Dict[str, int], override: object) -> None: if not isinstance(override, dict): return for name, index in override.items(): if name in target: try: target[name] = int(index) except (TypeError, ValueError): pass def _init_transport(self) -> None: if self.backend == "daemon": daemon_client_cls = _load_daemon_client_api() self.daemon_client = daemon_client_cls() return control_defaults, session_cls = _load_omnisocket_api() self.session = session_cls() self.session.connect( server_addr=self.server_addr, peer_id=self.peer_id, relay_via=self.relay_via, bind_ip=self.bind_ip, bind_device=self.bind_device, **control_defaults, ) def _joy_callback(self, msg: Joy) -> None: axes = list(msg.axes) + [0.0] * 16 buttons = list(msg.buttons) + [0] * 32 state = { "a": self._button_value(buttons, "a"), "x": self._button_value(buttons, "x"), "y": self._button_value(buttons, "y"), "start": self._button_value(buttons, "start"), "lx": self._axis_value(axes, "lx"), "ly": self._axis_value(axes, "ly"), "rx": self._axis_value(axes, "rx"), "dpad_h": self._axis_value(axes, "dpad_h"), } self._send_mode_events(state) self._send_trim_event(state) self._send_lift_events(state) self._send_analog_events(state) self.last_buttons = { name: int(state[name]) for name in ("a", "x", "y", "start") } self.last_dpad_h = float(state["dpad_h"]) def _button_value(self, buttons: list[int], name: str) -> int: index = self.button_map[name] return int(buttons[index]) if index < len(buttons) else 0 def _axis_value(self, axes: list[float], name: str) -> float: index = self.axis_map[name] return float(axes[index]) if index < len(axes) else 0.0 def _send_mode_events(self, state: Dict[str, float]) -> None: if self._rising_edge(state, "y"): self._send_event("pose_hold", "y") elif self._rising_edge(state, "x"): self._send_event("pose_home", "x") elif self._rising_edge(state, "a"): self._send_event("mode_stride", "a") def _send_trim_event(self, state: Dict[str, float]) -> None: if self._rising_edge(state, "start"): self._send_event("trim_reset", "start") def _send_lift_events(self, state: Dict[str, float]) -> None: dpad_h = float(state["dpad_h"]) if dpad_h <= -self.dpad_threshold and self.last_dpad_h > -self.dpad_threshold: self._send_event("lift_up", "dpad_left") elif dpad_h >= self.dpad_threshold and self.last_dpad_h < self.dpad_threshold: self._send_event("lift_down", "dpad_right") def _send_analog_events(self, state: Dict[str, float]) -> None: surge = self._compute_surge(state["ly"]) sway = self._cleanup_command( self._apply_deadzone(state["lx"]) * -0.4 + self.lateral_command_offset ) spin = self._cleanup_command( self._apply_deadzone(state["rx"]) * -0.4 + self.rotation_command_offset ) self._send_event("set_surge", "left_stick_y", surge) self._send_event("set_sway", "left_stick_x", sway) self._send_event("set_spin", "right_stick_x", spin) def _compute_surge(self, ly: float) -> float: ly = self._apply_deadzone(ly) if ly >= 0.0: value = ly * 0.8 + self.forward_command_offset else: value = ly * 0.5 return self._cleanup_command(value) def _apply_deadzone(self, value: float) -> float: if abs(value) < self.deadzone: return 0.0 return float(value) def _cleanup_command(self, value: float) -> float: if abs(value) < self.analog_epsilon: return 0.0 return float(value) def _rising_edge(self, state: Dict[str, float], name: str) -> bool: previous = int(self.last_buttons.get(name, 0)) return int(state[name]) == 1 and previous == 0 def _send_event( self, event_code: str, key_name: str, drive_value: float = 1.0 ) -> None: try: if self.backend == "daemon": assert self.daemon_client is not None result = self.daemon_client.send_control_event( source="xbox-sender", event_code=event_code, drive_value=drive_value, client_time_ms=int(time.time() * 1000), ) self.get_logger().debug( f"sent seq={result.get('assigned_seq_id')} event={event_code} key={key_name}" ) else: if self.session is None: return packet = make_control_packet(self.seq_id, event_code, drive_value) self.seq_id += 1 self.session.send(to=self.target_peer, data=packet.encode()) self.get_logger().debug( f"sent seq={packet.seq_id} event={event_code} key={key_name}" ) self._last_transport_error = "" except Exception as error: self._report_transport_error(str(error)) def _report_transport_error(self, message: str) -> None: now = time.monotonic() if ( message != self._last_transport_error or now - self._last_transport_error_at >= 2.0 ): self.get_logger().warning(f"control send failed via {self.backend}: {message}") self._last_transport_error = message self._last_transport_error_at = now def main(args: list[str] | None = None) -> None: rclpy.init(args=args) node = OmniSocketXboxSender() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() if __name__ == "__main__": main()