GR00T-WholeBodyControl / external_dependencies /unitree_sdk2_python /example /wireless_controller /wireless_controller.py
| 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) |