update wireless_controller example
This commit is contained in:
xiaoliangstd
parent
2991deae4e
commit
86755a4440
|
|
@ -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
|
||||
|
||||
# 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]
|
||||
|
||||
|
||||
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],
|
||||
]
|
||||
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)
|
||||
|
||||
|
||||
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)
|
||||
if __name__ == '__main__':
|
||||
|
||||
#Update key state
|
||||
for i in range(16):
|
||||
key_state[i][1] = (msg.keys & (1 << i)) >> i
|
||||
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
|
||||
input("Press Enter to continue...")
|
||||
|
||||
print(key_state)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
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)
|
||||
Loading…
Reference in New Issue