新增多设备控制

Deploy_Tienkung/common/joystick.py

@@ -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")