完善夹爪界面、跨平台打包与运行日志

gripper/control/controller.py

@@ -1,5 +1,6 @@
 import time
 import threading
+from collections import deque
 
 from ..common.filters import clamp
 
@@ -22,7 +23,7 @@ class SideTriggerGripController:
         self.status_callback = status_callback
         self._stop_event = threading.Event()
         self._manual_lock = threading.Lock()
-        self._manual_command = None
+        self._manual_commands = deque()
         self._mode_lock = threading.Lock()
         self._control_mode = "smart"
 
@@ -47,7 +48,13 @@ class SideTriggerGripController:
             "position": position,
         }
         with self._manual_lock:
-            self._manual_command = payload
+            if command == "move_position":
+                self._manual_commands = deque(
+                    item
+                    for item in self._manual_commands
+                    if item.get("command") != "move_position"
+                )
+            self._manual_commands.append(payload)
 
     def set_control_mode(self, mode):
         if mode not in {"smart", "manual"}:
@@ -61,9 +68,9 @@ class SideTriggerGripController:
 
     def _pop_manual_command(self):
         with self._manual_lock:
-            command = self._manual_command
-            self._manual_command = None
-        return command
+            if not self._manual_commands:
+                return None
+            return self._manual_commands.popleft()
 
     def run(self):
         self._stop_event.clear()
@@ -828,9 +835,20 @@ class SideTriggerGripController:
         action,
         trigger_armed,
     ):
+        left_fps = 0.0
+        right_fps = 0.0
+        try:
+            if len(self.reader.latest) >= 2:
+                left_sample = self.reader.latest[0] or {}
+                right_sample = self.reader.latest[1] or {}
+                left_fps = float(left_sample.get("fps", 0.0) or 0.0)
+                right_fps = float(right_sample.get("fps", 0.0) or 0.0)
+        except (AttributeError, TypeError, ValueError):
+            pass
         print(
             f"L={feedback.left_normal:7.3f}{self.normal_unit} "
             f"R={feedback.right_normal:7.3f}{self.normal_unit} "
+            f"fps=({left_fps:5.1f},{right_fps:5.1f}) "
             f"maxN={feedback.max_normal:7.3f}{self.normal_unit} "
             f"shear={feedback.max_shear:7.3f}{self.shear_unit} "
             f"trigger={int(trigger)} armed={int(trigger_armed)} "