def node():
global JOY_STATUS, LEGS_STATUS
print 'Starting Gait node:'
print 'Subscribing:'
print ' joy_status'
print 'Publishing:'
print ' legs_command'
rospy.Subscriber("joy_status", JoyStatus, joy_status_callback)
rospy.Subscriber("legs_status", LegsAngles, legs_status_callback)
plc = rospy.Publisher('legs_command', LegsAngles)
rospy.init_node('hex_gait_ctrl')
PUB_LEGS_COMMAND = plc
lc = LegsAngles()
lc.speed = 750
mc = ManualCtrl()
while not rospy.is_shutdown():
if JOY_STATUS != None:
js = JOY_STATUS
JOY_STATUS = None
if LEGS_STATUS != None:
ls = LEGS_STATUS
LEGS_STATUS = None
servo_arr = mc.gen( 0, 0, 0, 0, 0, 0 )
leg_angles_from_array( lc, servo_arr )
PUB_LEGS_COMMAND.publish(lc)
rospy.sleep(0.1)
'''
val = gg.gen_state( ST_REST )
leg_angles_from_array( lc, val )
PUB_LEGS_COMMAND.publish(lc)
rospy.sleep(5.0)
val = gg.gen_state( ST_MIDDLE_b )
leg_angles_from_array( lc, val )
PUB_LEGS_COMMAND.publish(lc)
rospy.sleep(5.0)
'''
'''
def node():
global JOY_STATUS, LEGS_STATUS
print 'Starting Gait node:'
print 'Subscribing:'
print ' joy_status'
print 'Publishing:'
print ' legs_command'
rospy.Subscriber("joy_status", JoyStatus, joy_status_callback)
rospy.Subscriber("legs_status", LegsAngles, legs_status_callback)
plc = rospy.Publisher('legs_command', LegsAngles)
rospy.init_node('hex_gait_ctrl')
PUB_LEGS_COMMAND = plc
gaitFSM = GaitFSM()
gait_gen = GaitGenerator()
req = GaitRequest()
req.legs_not_moving = True
lc = LegsAngles()
lc.speed = 750
override_legs_moving = False
time_state_changed = None
while not rospy.is_shutdown():
if JOY_STATUS != None:
js = JOY_STATUS
JOY_STATUS = None
update_request_joy( req, js )
if LEGS_STATUS != None:
ls = LEGS_STATUS
LEGS_STATUS = None
update_request_legs( req, ls )
if override_legs_moving:
req.legs_not_moving = False
state_changed = gaitFSM.execute_fsm(req)
if state_changed:
#req.legs_not_moving = False
override_legs_moving = True
time_state_changed = datetime.utcnow()
if time_state_changed != None and \
datetime.utcnow() - time_state_changed > timedelta(milliseconds=100):
override_legs_moving = False
#req.legs_not_moving = True
#time_state_changed = None
if state_changed:
servo_arr = gait_gen.gen_state( gaitFSM.state )
leg_angles_from_array( lc, servo_arr )
PUB_LEGS_COMMAND.publish(lc)
rospy.sleep(0.1)
'''
val = gg.gen_state( ST_REST )
leg_angles_from_array( lc, val )
PUB_LEGS_COMMAND.publish(lc)
rospy.sleep(5.0)
val = gg.gen_state( ST_MIDDLE_b )
leg_angles_from_array( lc, val )
PUB_LEGS_COMMAND.publish(lc)
rospy.sleep(5.0)
'''
'''
