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) ''' '''