meiqi/tienkung-szu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增多设备控制

Browse Source
This commit is contained in:
meiqi
2026-03-30 14:22:53 +08:00
parent 2988d4174b
commit 32702e25f3
7 changed files with 234 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
Deploy_Tienkung/common/joystick.py
View File

@@ -3,6 +3,7 @@ Joystick Control Module
Python equivalent of the C++ Joystick functionality for ROS Joy messages
"""
import os
import time
import yaml
import threading
from dataclasses import dataclass
@@ -76,6 +77,8 @@ class JoystickHumanoid:
self.max_yaw_speed = 0.0
# 高度平滑控制
self.target_height = 0.0
self.last_input_time = 0.0
self.last_fsm_command_time = 0.0
# 加载配置文件
self._load_config()
@@ -139,11 +142,14 @@ class JoystickHumanoid:
x2=msg.axes[0] if len(msg.axes) > 1 else 0.0,
y1=msg.axes[2] if len(msg.axes) > 2 else 0.0,
y2=msg.axes[1] if len(msg.axes) > 0 else 0.0)
if yunzhuo_map != self.joy_map:
self.last_input_time = time.time()
self.joy_map = yunzhuo_map
def joy_flag_update(self):
"""根据手柄输入更新控制标志"""
with self.data_mutex:
fsm_command_updated = False
# 更新手柄启动标志
if self.joy_map.f == -1.0:
self.joy_flag.enable = False
@@ -152,14 +158,17 @@ class JoystickHumanoid:
# FSM状态切换命令
if self.joy_map.c == 1.0:
self.joy_flag.fsm_state_command = "gotoSTOP"
fsm_command_updated = True
else:
button_pressed_nums = self.check_button_pressed_nums(
self.joy_map)
if button_pressed_nums == 0:
if self.joy_map.d == 1.0:
self.joy_flag.fsm_state_command = "gotoZERO"
fsm_command_updated = True
elif self.joy_map.a == 1.0:
self.joy_flag.fsm_state_command = "gotoWALKAMP"
fsm_command_updated = True
# 获取walk速度命令
self.get_x_y_yaw_speed_command()
# 获取高度命令
@@ -169,8 +178,13 @@ class JoystickHumanoid:
#e上拨
if self.joy_map.a == 1.0:
self.joy_flag.fsm_state_command = "gotoBEYONDMIMIC"
fsm_command_updated = True
elif self.joy_map.d == 1.0:
self.joy_flag.fsm_state_command = "gotoBEYONDZERO"
fsm_command_updated = True
if fsm_command_updated:
self.last_fsm_command_time = time.time()
@@ -179,6 +193,12 @@ class JoystickHumanoid:
with self.data_mutex:
return self.joy_flag
def get_last_input_time(self) -> float:
return self.last_input_time
def get_last_fsm_command_time(self) -> float:
return self.last_fsm_command_time
def init(self) -> int:
"""初始化手柄控制器"""
print("Joystick controller initialized")

21
Deploy_Tienkung/common/stdin_keyboard_control.py
View File

@@ -8,6 +8,7 @@ import select
import termios
import tty
import os
import time
import yaml
from typing import Optional
from .joystick import ControlFlag
@@ -50,6 +51,8 @@ class KeyboardController:
self.running = False
self.input_thread = None
self.original_terminal_settings = None
self.last_input_time = 0.0
self.last_fsm_command_time = 0.0
# 加载配置文件
self._load_config()
@@ -166,6 +169,7 @@ class KeyboardController:
def _process_key(self, key):
"""处理按键输入"""
handled = True
if key == 'w':
self._on_w_key()
elif key == 's':
@@ -207,7 +211,10 @@ class KeyboardController:
self._handle_arrow_key()
else:
# 忽略其他按键
pass
handled = False
if handled:
self.last_input_time = time.time()
def _handle_arrow_key(self):
"""处理方向键序列"""
@@ -296,17 +303,20 @@ class KeyboardController:
"""处理z键 - 切换到ZERO状态"""
with self.data_mutex:
self.keyboard_flag.fsm_state_command = "gotoZERO"
self.last_fsm_command_time = time.time()
print("Command: gotoZERO")
def _on_v_key(self):
"""处理v键 - 切换到BEYONGDMIMIC状态"""
with self.data_mutex:
self.keyboard_flag.fsm_state_command = "gotoBEYONDMIMIC"
self.last_fsm_command_time = time.time()
print("Command: gotoBEYONDMIMIC")
def _on_c_key(self):
"""处理c键 - 切换到STOP状态"""
with self.data_mutex:
self.keyboard_flag.fsm_state_command = "gotoSTOP"
self.last_fsm_command_time = time.time()
print("Command: gotoSTOP")
@@ -346,6 +356,7 @@ class KeyboardController:
"""处理m键 - 切换到WALKAMP状态"""
with self.data_mutex:
self.keyboard_flag.fsm_state_command = "gotoWALKAMP"
self.last_fsm_command_time = time.time()
print("Command: gotoWALKAMP")
def _on_p_key(self):
@@ -355,6 +366,7 @@ class KeyboardController:
self.keyboard_flag.y_speed_command = 0.0
self.keyboard_flag.yaw_speed_command = 0.0
self.keyboard_flag.fsm_state_command = "gotoMYPOLICY"
self.last_fsm_command_time = time.time()
print("Command: gotoMYPOLICY (movement commands reset to zero)")
def _on_n_key(self):
@@ -364,6 +376,7 @@ class KeyboardController:
self.keyboard_flag.y_speed_command = 0.0
self.keyboard_flag.yaw_speed_command = 0.0
self.keyboard_flag.fsm_state_command = "gotoXSIMRUN"
self.last_fsm_command_time = time.time()
print("Command: gotoXSIMRUN (movement commands reset to zero)")
def _handle_ctrl_c(self):
@@ -418,6 +431,12 @@ class KeyboardController:
flag_copy = KeyboardFlag()
flag_copy.__dict__.update(self.keyboard_flag.__dict__)
return flag_copy
def get_last_input_time(self) -> float:
return self.last_input_time
def get_last_fsm_command_time(self) -> float:
return self.last_fsm_command_time
def init(self) -> int:
"""初始化键盘控制器"""

54
Deploy_Tienkung/common/xbox_control.py
View File

@@ -3,6 +3,7 @@ XBOX Controller compatibility layer.
Implements the same FSM modes and control flags as `stdin_keyboard_control.py` / `joystick.py`.
"""
import os
import time
import yaml
import threading
from typing import Optional
@@ -53,6 +54,8 @@ class XBOXController:
self.map = XBOXMap()
self.flag = XBOXFlag()
self.data_mutex = threading.Lock()
self.last_input_time = 0.0
self.last_fsm_command_time = 0.0
# state tracking
self.last_select = 0
@@ -130,44 +133,51 @@ class XBOXController:
# axes layout may differ; try safe indexing
axes = list(msg.axes) + [0.0] * 16
buttons = list(msg.buttons) + [0] * 32
# common mapping assumptions (best-effort)
self.map.lx = axes[self.axis_map['lx']]
self.map.ly = axes[self.axis_map['ly']]
self.map.rx = axes[self.axis_map['rx']]
self.map.ry = axes[self.axis_map['ry']]
# triggers sometimes on axes
self.map.l_trigger = axes[self.axis_map['l_trigger']]
self.map.r_trigger = axes[self.axis_map['r_trigger']]
# dpad may be on axes
self.map.dpad_h = axes[self.axis_map['dpad_h']]
self.map.dpad_v = axes[self.axis_map['dpad_v']]
# buttons using button_map indices
new_map = XBOXMap(
lx=axes[self.axis_map['lx']],
ly=axes[self.axis_map['ly']],
rx=axes[self.axis_map['rx']],
ry=axes[self.axis_map['ry']],
l_trigger=axes[self.axis_map['l_trigger']],
r_trigger=axes[self.axis_map['r_trigger']],
dpad_h=axes[self.axis_map['dpad_h']],
dpad_v=axes[self.axis_map['dpad_v']],
)
for name, idx in self.button_map.items():
try:
val = buttons[idx]
except Exception:
val = 0
setattr(self.map, name, val)
setattr(new_map, name, val)
if new_map != self.map:
self.last_input_time = time.time()
self.map = new_map
def xbox_flag_update(self):
"""Update ControlFlag from the xbox map, mirroring joystick logic."""
with self.data_mutex:
fsm_command = None
# FSM state mapping - cover keyboard commands z/c/m/h/g/p/o
# c -> gotoSTOP
if self.map.y == 1:
self.flag.fsm_state_command = 'gotoSTOP'
fsm_command = 'gotoSTOP'
# a -> gotoWALKAMP
elif self.map.a == 1:
self.flag.fsm_state_command = 'gotoWALKAMP'
fsm_command = 'gotoWALKAMP'
# h -> gotoDH (Left trigger + A)
# v -> gotoBEYONDMIMIC (Left trigger + home)
elif self.map.l_trigger < -0.5 and self.map.home == 1:
self.flag.fsm_state_command = 'gotoBEYONDMIMIC'
fsm_command = 'gotoBEYONDMIMIC'
# z -> gotoZERO
elif self.map.x == 1:
self.flag.fsm_state_command = 'gotoZERO'
fsm_command = 'gotoZERO'
if fsm_command is not None:
self.flag.fsm_state_command = fsm_command
self.last_fsm_command_time = time.time()
# detect state change
if not hasattr(self, '_last_state'):
@@ -251,6 +261,12 @@ class XBOXController:
with self.data_mutex:
return self.flag
def get_last_input_time(self) -> float:
return self.last_input_time
def get_last_fsm_command_time(self) -> float:
return self.last_fsm_command_time
def init(self) -> int:
print("XBOX controller initialized")
return 0