1.增加xbox控制器和飞行手柄控制器

2.增加多臂多控制器模式
3.末端姿态由欧拉角控制切换到四元数控制
4.增加vr手柄控制器

Signed-off-by: 1002142102@qq.com <1002142102@qq.com>

robot_client/utlis/pygame_event.py

@@ -0,0 +1,81 @@
+import threading
+import pygame
+import atexit
+
+class PygameEventState(object):
+    def __init__(self,joystick):
+        joystick.init()
+        self.states={
+            'axis':[0]*joystick.get_numaxes(),
+            'button':[0]*joystick.get_numbuttons(),
+            'hat':[[0,0]]*joystick.get_numhats()
+        }
+    def get_axis(self,index):
+        return self.states['axis'][index]
+    def get_numaxes(self):
+        return len(self.states['axis'])
+    def set_axis(self,index,value):
+        self.states['axis'][index]=value
+    
+    def get_button(self,index):
+        return self.states['button'][index]
+    def get_numbuttons(self):
+        return len(self.states['button'])
+    def set_button(self,index,value):
+        self.states['button'][index]=value
+    def get_hat(self,index):
+        return self.states['hat'][index]
+    def get_numhats(self):
+        return len(self.states['hat'])
+    def set_hat(self,index,value):
+        self.states['hat'][index]=value
+            
+
+class _PygameEvent:
+    def __init__(self):
+        self.running = False
+
+    def start(self):
+        if self.running: return
+        self.running = True
+        threading.Thread(target=self.eventLoop).start()
+
+    def getState(self,index):
+        if index >= len(self.states):
+            raise ValueError("不存在的控制器")
+        return self.states[index]
+    def eventLoop(self):
+        # 初始化pygame
+        pygame.init()
+        pygame.joystick.init()
+        """检测连接的手柄"""
+        joystick_count = pygame.joystick.get_count()
+        self.states=[]
+        joysticks = []
+        for i in range(joystick_count):
+            joystick = pygame.joystick.Joystick(i)
+            joystick.init()
+            self.states.append(PygameEventState(joystick))
+            joysticks.append(joystick)
+
+        print(f"检测到 {joystick_count} 个手柄设备")
+        while self.running:
+            event = pygame.event.wait()
+            data = event.dict
+            if 'instance_id' not in data:
+                continue
+            state = self.states[data['instance_id']]
+            if event.type == pygame.JOYAXISMOTION:
+                state.set_axis(data['axis'], data['value'])
+            elif event.type == pygame.JOYHATMOTION:
+                state.set_hat(data['hat'], data['value'])
+            elif event.type == pygame.JOYBUTTONUP:
+                state.set_button(data['button'], 0)
+            elif event.type == pygame.JOYBUTTONDOWN:
+                state.set_button(data['button'], 1)
+    def stop(self):
+        self.running = False
+@atexit.register
+def exit():
+    PygameEvent.stop()
+PygameEvent = _PygameEvent()