28 lines
824 B
Python
28 lines
824 B
Python
|
import time
|
||
|
|
import sys
|
||
|
|
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactortyInitialize
|
||
|
|
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_
|
||
|
|
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
|
||
|
|
|
||
|
|
import unitree_legged_const as go2
|
||
|
|
|
||
|
|
|
||
|
|
def LowStateHandler(msg: LowState_):
|
||
|
|
|
||
|
|
# print front right hip motor states
|
||
|
|
print("FR_0 motor state: ", msg.motor_state[go2.LegID["FR_0"]])
|
||
|
|
print("IMU state: ", msg.imu_state)
|
||
|
|
print("Battery state: voltage: ", msg.power_v, "current: ", msg.power_a)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
if len(sys.argv)>1:
|
||
|
|
ChannelFactortyInitialize(0, sys.argv[1])
|
||
|
|
else:
|
||
|
|
ChannelFactortyInitialize(0)
|
||
|
|
sub = ChannelSubscriber("rt/lowstate", LowState_)
|
||
|
|
sub.Init(LowStateHandler, 10)
|
||
|
|
|
||
|
|
while True:
|
||
|
|
time.sleep(10.0)
|