25 lines
812 B
Python
25 lines
812 B
Python
import time
|
|
import sys
|
|
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
|
|
|
from unitree_sdk2py.idl.default import unitree_go_msg_dds__SportModeState_
|
|
from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
|
|
|
|
|
|
def HighStateHandler(msg: SportModeState_):
|
|
print("Position: ", msg.position)
|
|
print("Velocity: ", msg.velocity)
|
|
print("Yaw velocity: ", msg.yaw_speed)
|
|
print("Foot position in body frame: ", msg.foot_position_body)
|
|
print("Foot velocity in body frame: ", msg.foot_speed_body)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# sys.argv[1]: name of the network interface
|
|
ChannelFactoryInitialize(0, sys.argv[1])
|
|
sub = ChannelSubscriber("rt/sportmodestate", SportModeState_)
|
|
sub.Init(HighStateHandler, 10)
|
|
|
|
while True:
|
|
time.sleep(10.0)
|