sync recent changes

additional_functions/sniff.py

@@ -0,0 +1,58 @@
+import time, threading, numpy as np
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd
+
+TOPIC = "rt/lowcmd"
+N = 35
+
+class LowCmdSniffer:
+    def __init__(self, iface="en7"):
+        ChannelFactoryInitialize(0, iface)
+        self.sub = ChannelSubscriber(TOPIC, HGLowCmd)
+        # Use a queue depth; passing None as handler is fine for polling
+        self.sub.Init(None, 20)
+
+        self._lock = threading.Lock()
+        self._have = False
+        self.q   = np.zeros(N)
+        self.dq  = np.zeros(N)
+        self.kp  = np.zeros(N)
+        self.kd  = np.zeros(N)
+        self.tau = np.zeros(N)
+
+        threading.Thread(target=self._loop, daemon=True).start()
+
+    def _loop(self):
+        while True:
+            msg = self.sub.Read()
+            if msg is not None:
+                with self._lock:
+                    for i in range(N):
+                        mc = msg.motor_cmd[i]
+                        self.q[i]   = getattr(mc, "q", 0.0)
+                        self.dq[i]  = getattr(mc, "qd", 0.0)  # named qd in HG
+                        self.kp[i]  = getattr(mc, "kp", 0.0)
+                        self.kd[i]  = getattr(mc, "kd", 0.0)
+                        self.tau[i] = getattr(mc, "tau", 0.0)
+                    self._have = True
+            time.sleep(0.001)
+
+    def latest(self):
+        with self._lock:
+            return self._have, self.q.copy(), self.dq.copy(), self.kp.copy(), self.kd.copy(), self.tau.copy()
+
+if __name__ == "__main__":
+    sniffer = LowCmdSniffer(iface="en7")
+    t0 = time.time()
+    while time.time() - t0 < 5:
+        ok, q, dq, kp, kd, tau = sniffer.latest()
+        if ok:
+            print("kp :", kp.round(2))
+            print("kd :", kd.round(2))
+            print("q  :", q.round(4))
+            print("dq :", dq.round(4))
+            print("tau:", tau.round(3))
+            break
+        time.sleep(0.01)
+    else:
+        print("No LowCmd messages seen on rt/lowcmd in 5s (check topic/type/interface).")