File size: 3,917 Bytes
d1e2d14 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 | import time
import sys
import struct
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
# 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
# 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 parse(self,remoteData):
self.parse_key(remoteData)
self.parse_botton(remoteData[2],remoteData[3])
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__':
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)
custom = Custom()
custom.Init()
while True:
time.sleep(1) |