新增多设备控制

2026-03-30 14:22:53 +08:00
parent 2988d4174b
commit 32702e25f3
7 changed files with 234 additions and 59 deletions
--- a/Deploy_Tienkung/rl_control_node.py
+++ b/Deploy_Tienkung/rl_control_node.py
@@ -47,6 +47,8 @@ def timing_decorator(func):
 class XMIGCSControlNode(Node):
    """xMIGCS控制节点Python版本"""

+    VALID_CONTROL_TOOLS = ("joystick", "xbox", "keyboard", "udp_loopback")
+
    def __init__(self, debug=False):
        super().__init__('xmigcs_control_node')

@@ -95,7 +97,8 @@ class XMIGCSControlNode(Node):
        print(self.config)

        # 获取控制器类型
-        self.control_tool = self.config.get('control_tool', 'keyboard')
+        raw_control_tool = self.config.get('control_tool', 'keyboard')
+        self.control_tools = self._normalize_control_tools(raw_control_tool)
        # 提取关键配置参数
        self.motor_num = self.config.get('motor_num')
        self.dt = self.config.get('dt')
@@ -107,6 +110,34 @@ class XMIGCSControlNode(Node):
        if self.sim and user_name == 'ubuntu':
            raise RuntimeError("On ubuntu user, sim must be set to false")

+    def _normalize_control_tools(self, raw_control_tool):
+        """Normalize control_tool config to an ordered tool list."""
+        if isinstance(raw_control_tool, str):
+            normalized = raw_control_tool.replace("+", ",")
+            requested_tools = [
+                item.strip() for item in normalized.split(",") if item.strip()
+            ]
+        elif isinstance(raw_control_tool, list):
+            requested_tools = [str(item).strip() for item in raw_control_tool if str(item).strip()]
+        else:
+            raise ValueError("control_tool must be a string or list")
+
+        if not requested_tools:
+            requested_tools = ["keyboard"]
+
+        deduped_tools = []
+        for tool_name in requested_tools:
+            if tool_name not in self.VALID_CONTROL_TOOLS:
+                raise ValueError(
+                    f"Unsupported control tool '{tool_name}'. "
+                    f"Expected one of {self.VALID_CONTROL_TOOLS}"
+                )
+            if tool_name not in deduped_tools:
+                deduped_tools.append(tool_name)
+
+        print(f"[control] active tools: {deduped_tools}")
+        return deduped_tools
+
    def _init_data_structures(self):
        """初始化数据结构"""
        # 机器人数据
@@ -117,6 +148,7 @@ class XMIGCSControlNode(Node):
        self.queue_joy_cmd = queue.Queue(maxsize=1)
        self.queue_xbox_cmd = queue.Queue(maxsize=1)
        self.control_flag = ControlFlag()
+        self.last_applied_fsm_command_time = 0.0

    def _init_ros_interfaces(self):
        """初始化ROS接口（仅非电机相关）"""
@@ -125,10 +157,10 @@ class XMIGCSControlNode(Node):
                                 depth=5)

        # 订阅者（非电机相关）
-        if self.control_tool == "joystick":
+        if "joystick" in self.control_tools:
            self.sub_joy_cmd = self.create_subscription(
                Joy, '/sbus_data', self._joy_callback, qos_profile)
-        if self.control_tool == "xbox":
+        if "xbox" in self.control_tools:
            self.sub_xbox_cmd = self.create_subscription(
                Joy, '/xbox_data', self._xbox_callback, qos_profile)

@@ -136,25 +168,25 @@ class XMIGCSControlNode(Node):
        """初始化控制系统"""

        # 手柄控制器
-        if self.control_tool == "joystick":
+        if "joystick" in self.control_tools:
            self.joystick_humanoid = JoystickHumanoid()
            self.joystick_humanoid.init()

        # 键盘控制器
-        if self.control_tool == "keyboard":
+        if "keyboard" in self.control_tools:
            self.keyboard_controller = KeyboardController()
            self.keyboard_controller.init()
            # 如果使用键盘控制，启动键盘监听
            self.keyboard_controller.start()

        # UDP控制器
-        if self.control_tool == "udp_loopback":
+        if "udp_loopback" in self.control_tools:
            self.udp_fsm_controller = UDPFSMController()
            self.udp_fsm_controller.init()
            self.udp_fsm_controller.start()

        # Xbox控制器
-        if self.control_tool == "xbox":
+        if "xbox" in self.control_tools:
            self.xbox_controller = XBOXController()
            self.xbox_controller.init()

@@ -247,8 +279,10 @@ class XMIGCSControlNode(Node):

    # @timing_decorator
    def _process_controller_data(self):
+        source_flags = []
+
        # 处理控制器输入
-        if self.control_tool == "joystick":
+        if "joystick" in self.control_tools:
            # 处理手柄输入
            while not self.queue_joy_cmd.empty():
                try:
@@ -258,7 +292,14 @@ class XMIGCSControlNode(Node):
                    break
                except queue.Empty:
                    break
-        if self.control_tool == "xbox":
+            source_flags.append((
+                "joystick",
+                self.joystick_humanoid.get_joy_flag(),
+                self.joystick_humanoid.get_last_input_time(),
+                self.joystick_humanoid.get_last_fsm_command_time(),
+            ))
+
+        if "xbox" in self.control_tools:
            while not self.queue_xbox_cmd.empty():
                try:
                    msg = self.queue_xbox_cmd.get_nowait()
@@ -267,22 +308,78 @@ class XMIGCSControlNode(Node):
                    break
                except queue.Empty:
                    break
+            source_flags.append((
+                "xbox",
+                self.xbox_controller.get_xbox_flag(),
+                self.xbox_controller.get_last_input_time(),
+                self.xbox_controller.get_last_fsm_command_time(),
+            ))

-        if self.control_tool == "keyboard":
+        if "keyboard" in self.control_tools:
            self.keyboard_controller.update_flag()
-            flag = self.keyboard_controller.get_keyboard_flag()
-        elif self.control_tool == "udp_loopback":
+            source_flags.append((
+                "keyboard",
+                self.keyboard_controller.get_keyboard_flag(),
+                self.keyboard_controller.get_last_input_time(),
+                self.keyboard_controller.get_last_fsm_command_time(),
+            ))
+
+        if "udp_loopback" in self.control_tools:
            self.udp_fsm_controller.update_flag()
-            flag = self.udp_fsm_controller.get_udp_flag()
-        elif self.control_tool == "joystick":
-            flag = self.joystick_humanoid.get_joy_flag()
-        elif self.control_tool == "xbox":
-            flag = self.xbox_controller.get_xbox_flag()
-        else:
-            print("[ERROR] No control tool specified")
-        print('*' * 30 + f"current flag: {flag}" + '*' * 30)
+            source_flags.append((
+                "udp_loopback",
+                self.udp_fsm_controller.get_udp_flag(),
+                self.udp_fsm_controller.get_last_input_time(),
+                self.udp_fsm_controller.get_last_fsm_command_time(),
+            ))
+
+        source_name, active_flag = self._select_control_flag(source_flags)
+        flag = self._copy_control_flag(active_flag)
+        flag.fsm_state_command = self._select_fsm_command(source_flags)
+        print('*' * 30 + f"current flag source={source_name}: {flag}" + '*' * 30)
        self.control_flag = flag

+    def _select_control_flag(self, source_flags):
+        """Select the most recently active controller."""
+        if not source_flags:
+            print("[ERROR] No control tool specified")
+            return "none", ControlFlag()
+
+        selected_source = source_flags[0]
+        for candidate in source_flags[1:]:
+            if candidate[2] > selected_source[2]:
+                selected_source = candidate
+
+        return selected_source[0], selected_source[1]
+
+    def _copy_control_flag(self, flag: ControlFlag) -> ControlFlag:
+        flag_copy = type(flag)()
+        flag_copy.__dict__.update(flag.__dict__)
+        return flag_copy
+
+    def _select_fsm_command(self, source_flags) -> str:
+        latest_source = None
+        for candidate in source_flags:
+            if latest_source is None or candidate[3] > latest_source[3]:
+                latest_source = candidate
+
+        if latest_source is not None and latest_source[3] > self.last_applied_fsm_command_time:
+            self.last_applied_fsm_command_time = latest_source[3]
+            return latest_source[1].fsm_state_command
+
+        return self._get_hold_fsm_command()
+
+    def _get_hold_fsm_command(self) -> str:
+        state_to_command = {
+            FSMStateName.STOP: "gotoSTOP",
+            FSMStateName.ZERO: "gotoZERO",
+            FSMStateName.WALKAMP: "gotoWALKAMP",
+            FSMStateName.MYPOLICY: "gotoMYPOLICY",
+            FSMStateName.XSIMRUN: "gotoXSIMRUN",
+        }
+        current_state = self.robot_fsm.get_current_state()
+        return state_to_command.get(current_state, self.control_flag.fsm_state_command)
+
    # @timing_decorator
    def _update_robot_data(self, flag: ControlFlag, time_passed: float):
        """更新机器人数据"""
@@ -321,10 +418,10 @@ class XMIGCSControlNode(Node):
        self.control_running = False
        # # 先停止键盘控制器（重要！）
        if hasattr(self,
-                   'keyboard_controller') and self.control_tool == "keyboard":
+                   'keyboard_controller') and "keyboard" in self.control_tools:
            self.keyboard_controller.stop()
        if hasattr(self,
-                   'udp_fsm_controller') and self.control_tool == "udp_loopback":
+                   'udp_fsm_controller') and "udp_loopback" in self.control_tools:
            self.udp_fsm_controller.stop()

        if self.control_thread and self.control_thread.is_alive():