247 lines
8.4 KiB
Python
247 lines
8.4 KiB
Python
"""ROS2 Joy -> localhost UDP bridge for Xbox control."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import socket
|
|
from pathlib import Path
|
|
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 .protocol import InputEnvelope
|
|
except ImportError: # pragma: no cover - direct script execution fallback
|
|
from protocol import InputEnvelope
|
|
|
|
|
|
class UDPXboxSender(Node):
|
|
"""Subscribe to Joy messages and forward them as UDP loopback events."""
|
|
|
|
def __init__(self) -> None:
|
|
super().__init__("udp_xbox_sender")
|
|
self.config: Dict[str, object] = {}
|
|
self.seq_id = 0
|
|
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
self.last_buttons: Dict[str, int] = {}
|
|
self.last_dpad_h = 0.0
|
|
|
|
self._load_config()
|
|
|
|
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} -> udp://{self.target_host}:{self.target_port}"
|
|
)
|
|
self.get_logger().info(
|
|
"Buttons: A=WALKAMP X=ZERO Y=STOP START=reset"
|
|
)
|
|
|
|
def destroy_node(self) -> bool:
|
|
if self.socket is not None:
|
|
self.socket.close()
|
|
return super().destroy_node()
|
|
|
|
def _load_config(self) -> None:
|
|
udp_config_path = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
|
|
main_config_path = Path(__file__).resolve().parents[1] / "config" / "dex_config.yaml"
|
|
|
|
with udp_config_path.open("r", encoding="utf-8") as file:
|
|
udp_config = yaml.safe_load(file) or {}
|
|
with main_config_path.open("r", encoding="utf-8") as file:
|
|
main_config = yaml.safe_load(file) or {}
|
|
|
|
sender_cfg = udp_config.get("sender", {})
|
|
xbox_sender_cfg = udp_config.get("xbox_sender", {})
|
|
xbox_cfg = main_config.get("xbox", {})
|
|
|
|
self.target_host = sender_cfg.get("target_host", "127.0.0.1")
|
|
self.target_port = int(sender_cfg.get("target_port", 31000))
|
|
self.source_tag = xbox_sender_cfg.get("source_tag", "xbox_udp")
|
|
self.joy_topic = xbox_sender_cfg.get("joy_topic", "/xbox_data")
|
|
|
|
self.deadzone = float(xbox_sender_cfg.get("deadzone", 0.10))
|
|
self.analog_epsilon = float(xbox_sender_cfg.get("analog_epsilon", 0.01))
|
|
self.dpad_threshold = float(xbox_sender_cfg.get("dpad_threshold", 0.50))
|
|
self.trigger_pressed_threshold = float(
|
|
xbox_sender_cfg.get("trigger_pressed_threshold", -0.50)
|
|
)
|
|
|
|
self.forward_command_offset = float(
|
|
xbox_cfg.get("forward_command_offset", 0.0)
|
|
)
|
|
self.lateral_command_offset = float(
|
|
xbox_cfg.get("lateral_command_offset", 0.0)
|
|
)
|
|
self.rotation_command_offset = float(
|
|
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, xbox_sender_cfg.get("button_map"))
|
|
self._merge_mapping(self.axis_map, xbox_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 _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"),
|
|
"b": self._button_value(buttons, "b"),
|
|
"x": self._button_value(buttons, "x"),
|
|
"y": self._button_value(buttons, "y"),
|
|
"start": self._button_value(buttons, "start"),
|
|
"home": self._button_value(buttons, "home"),
|
|
"lx": self._axis_value(axes, "lx"),
|
|
"ly": self._axis_value(axes, "ly"),
|
|
"rx": self._axis_value(axes, "rx"),
|
|
"l_trigger": self._axis_value(axes, "l_trigger"),
|
|
"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", "b", "x", "y", "start", "home")
|
|
}
|
|
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:
|
|
envelope = InputEnvelope(
|
|
seq_id=self.seq_id,
|
|
event_code=event_code,
|
|
key_name=key_name,
|
|
drive_value=drive_value,
|
|
source_tag=self.source_tag,
|
|
)
|
|
self.seq_id += 1
|
|
self.socket.sendto(
|
|
envelope.encode(), (self.target_host, self.target_port)
|
|
)
|
|
|
|
|
|
def main(args: list[str] | None = None) -> None:
|
|
rclpy.init(args=args)
|
|
node = UDPXboxSender()
|
|
try:
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|