update wireless_controller example

Author: xiaoliangstd

example/wireless_controller/wireless_controller.py

@@ -1,55 +1,131 @@
 import time
 import sys
+import struct
+
 from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
 
-from unitree_sdk2py.idl.default import unitree_go_msg_dds__WirelessController_
-from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
+# Uncomment the following two lines when using Go2、Go2-W、B2、B2-W、H1 robot
+# from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
+# from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
+
+# Uncomment the following two lines when using G1、H1-2 robot
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
 
+class unitreeRemoteController:
+    def __init__(self):
+        # key
+        self.Lx = 0           
+        self.Rx = 0            
+        self.Ry = 0            
+        self.Ly = 0
 
-key_state = [
-    ["R1", 0],
-    ["L1", 0],
-    ["start", 0],
-    ["select", 0],
-    ["R2", 0],
-    ["L2", 0],
-    ["F1", 0],
-    ["F2", 0],
-    ["A", 0],
-    ["B", 0],
-    ["X", 0],
-    ["Y", 0],
-    ["up", 0],
-    ["right", 0],
-    ["down", 0],
-    ["left", 0],
-]
+        # button
+        self.L1 = 0
+        self.L2 = 0
+        self.R1 = 0
+        self.R2 = 0
+        self.A = 0
+        self.B = 0
+        self.X = 0
+        self.Y = 0
+        self.Up = 0
+        self.Down = 0
+        self.Left = 0
+        self.Right = 0
+        self.Select = 0
+        self.F1 = 0
+        self.F3 = 0
+        self.Start = 0
+       
+    def parse_botton(self,data1,data2):
+        self.R1 = (data1 >> 0) & 1
+        self.L1 = (data1 >> 1) & 1
+        self.Start = (data1 >> 2) & 1
+        self.Select = (data1 >> 3) & 1
+        self.R2 = (data1 >> 4) & 1
+        self.L2 = (data1 >> 5) & 1
+        self.F1 = (data1 >> 6) & 1
+        self.F3 = (data1 >> 7) & 1
+        self.A = (data2 >> 0) & 1
+        self.B = (data2 >> 1) & 1
+        self.X = (data2 >> 2) & 1
+        self.Y = (data2 >> 3) & 1
+        self.Up = (data2 >> 4) & 1
+        self.Right = (data2 >> 5) & 1
+        self.Down = (data2 >> 6) & 1
+        self.Left = (data2 >> 7) & 1
 
+    def parse_key(self,data):
+        lx_offset = 4
+        self.Lx = struct.unpack('<f', data[lx_offset:lx_offset + 4])[0]
+        rx_offset = 8
+        self.Rx = struct.unpack('<f', data[rx_offset:rx_offset + 4])[0]
+        ry_offset = 12
+        self.Ry = struct.unpack('<f', data[ry_offset:ry_offset + 4])[0]
+        L2_offset = 16
+        L2 = struct.unpack('<f', data[L2_offset:L2_offset + 4])[0] # Placeholder，unused
+        ly_offset = 20
+        self.Ly = struct.unpack('<f', data[ly_offset:ly_offset + 4])[0]
 
-def WirelessControllerHandler(msg: WirelessController_):
-    global key_state
-    print("lx: ", msg.lx)
-    print("lx: ", msg.ly)
-    print("lx: ", msg.rx)
-    print("lx: ", msg.ry)
-    print("keys: ", msg.keys)
 
-    #Update key state
-    for i in range(16):
-        key_state[i][1] = (msg.keys & (1 << i)) >> i
+    def parse(self,remoteData):
+        self.parse_key(remoteData)
+        self.parse_botton(remoteData[2],remoteData[3])
 
-    print(key_state)
+        print("debug unitreeRemoteController: ")
+        print("Lx:", self.Lx)
+        print("Rx:", self.Rx)
+        print("Ry:", self.Ry)
+        print("Ly:", self.Ly)
 
+        print("L1:", self.L1)
+        print("L2:", self.L2)
+        print("R1:", self.R1)
+        print("R2:", self.R2)
+        print("A:", self.A)
+        print("B:", self.B)
+        print("X:", self.X)
+        print("Y:", self.Y)
+        print("Up:", self.Up)
+        print("Down:", self.Down)
+        print("Left:", self.Left)
+        print("Right:", self.Right)
+        print("Select:", self.Select)
+        print("F1:", self.F1)
+        print("F3:", self.F3)
+        print("Start:", self.Start)
+        print("\n")
+
+        
+class Custom:
+    def __init__(self):
+        self.low_state = None 
+        self.remoteController = unitreeRemoteController()
+
+    def Init(self):
+        self.lowstate_subscriber = ChannelSubscriber("rt/lf/lowstate", LowState_)
+        self.lowstate_subscriber.Init(self.LowStateMessageHandler, 10)
+
+    
+    def LowStateMessageHandler(self, msg: LowState_):
+        self.low_state = msg
+        wireless_remote_data = self.low_state.wireless_remote
+        self.remoteController.parse(wireless_remote_data)
 
 
-if __name__ == "__main__":
+if __name__ == '__main__':
+
+    print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
+    input("Press Enter to continue...")
+
     if len(sys.argv)>1:
         ChannelFactoryInitialize(0, sys.argv[1])
     else:
         ChannelFactoryInitialize(0)
-        
-    sub = ChannelSubscriber("rt/wirelesscontroller", WirelessController_)
-    sub.Init(WirelessControllerHandler, 10)
-    
-    while True:
-        time.sleep(10.0)
+
+    custom = Custom()
+    custom.Init()
+
+    while True:   
+        time.sleep(1)