def rosShutdownHook():
mCtrl.ResetBodyhub()
rospy.signal_shutdown('node_close')
for i in range(0, 16):
mCtrl.SendGaitCommand(0.07, 0.0, -7.5)
for i in range(0, 16):
mCtrl.SendGaitCommand(0.07, 0.0, 7.5)
for i in range(0, 5):
mCtrl.SendGaitCommand(0.0, 0.0, -12.0)
def rosShutdownHook():
mCtrl.ResetBodyhub()
rospy.signal_shutdown('node_close')
if __name__ == '__main__':
rospy.init_node('walking_SPath_node', anonymous=True)
time.sleep(0.2)
rospy.on_shutdown(rosShutdownHook)
rospy.loginfo('node runing...')
if mCtrl.SetBodyhubTo_walking(nodeControlId) == False:
rospy.logerr('bodyhub to walking fail!')
rospy.signal_shutdown('error')
exit(1)
time.sleep(1)
while not rospy.is_shutdown():
walkingSPath()
mCtrl.WaitForWalkingDone()
mCtrl.ResetBodyhub()
rospy.signal_shutdown('exit')