90 lines
2.9 KiB
Python
90 lines
2.9 KiB
Python
"""键盘控制器 — 兼容 sim2sim/input_dev/keyboard.py 的接口与平滑参数。"""
|
|
import numpy as np
|
|
|
|
try:
|
|
from pynput import keyboard
|
|
PYNPUT_AVAILABLE = True
|
|
except ImportError:
|
|
PYNPUT_AVAILABLE = False
|
|
keyboard = None # type: ignore
|
|
|
|
|
|
class KeyboardCommandController:
|
|
"""方向键 + AD 键的键盘指令源。
|
|
指令: [vx, vy, yaw_rate]，平滑加减速；空格触发急停标志。
|
|
"""
|
|
|
|
def __init__(self,
|
|
max_x_vel: float = 0.8,
|
|
max_y_vel: float = 0.3,
|
|
max_yaw_vel: float = 0.5,
|
|
acc_step: float = 0.05,
|
|
dec_step: float = 0.1):
|
|
if not PYNPUT_AVAILABLE:
|
|
raise RuntimeError("pynput 不可用，无法使用键盘控制；改用其他输入源。")
|
|
|
|
self.current_cmd = np.zeros(3, dtype=np.float32)
|
|
self.max_x_vel = max_x_vel
|
|
self.max_y_vel = max_y_vel
|
|
self.max_yaw_vel = max_yaw_vel
|
|
self.acc_step = acc_step
|
|
self.dec_step = dec_step
|
|
|
|
self._pressed = set()
|
|
self._estop = False
|
|
self.listener = keyboard.Listener(
|
|
on_press=self._on_press, on_release=self._on_release
|
|
)
|
|
|
|
def start(self):
|
|
self.listener.start()
|
|
print("[Keyboard] 启动。↑↓ 前后, ←→ 转向, A/D 横移, SPACE 急停")
|
|
|
|
def stop(self):
|
|
try:
|
|
self.listener.stop()
|
|
except Exception:
|
|
pass
|
|
|
|
def _on_press(self, key):
|
|
self._pressed.add(key)
|
|
if key == keyboard.Key.space:
|
|
self._estop = True
|
|
|
|
def _on_release(self, key):
|
|
self._pressed.discard(key)
|
|
|
|
def is_estop_triggered(self) -> bool:
|
|
return self._estop
|
|
|
|
def reset_estop(self):
|
|
self._estop = False
|
|
|
|
def get_command(self) -> np.ndarray:
|
|
target = np.zeros(3, dtype=np.float32)
|
|
if keyboard.Key.up in self._pressed:
|
|
target[0] += self.max_x_vel
|
|
if keyboard.Key.down in self._pressed:
|
|
target[0] -= self.max_x_vel
|
|
if keyboard.Key.left in self._pressed:
|
|
target[2] += self.max_yaw_vel
|
|
if keyboard.Key.right in self._pressed:
|
|
target[2] -= self.max_yaw_vel
|
|
try:
|
|
if keyboard.KeyCode.from_char('a') in self._pressed:
|
|
target[1] += self.max_y_vel
|
|
if keyboard.KeyCode.from_char('d') in self._pressed:
|
|
target[1] -= self.max_y_vel
|
|
except Exception:
|
|
pass
|
|
|
|
for i, max_v in enumerate((self.max_x_vel, self.max_y_vel, self.max_yaw_vel)):
|
|
step = self.acc_step if target[i] != 0 else self.dec_step
|
|
if i == 2:
|
|
step *= 2.0
|
|
if self.current_cmd[i] < target[i]:
|
|
self.current_cmd[i] = min(self.current_cmd[i] + step, target[i])
|
|
else:
|
|
self.current_cmd[i] = max(self.current_cmd[i] - step, target[i])
|
|
return self.current_cmd.copy()
|