test_shifter.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Description:  Unit test for ART shifter interface to ioadr driver
00004 #
00005 #   Copyright (C) 2005, 2007, 2009 Austin Robot Technology
00006 #
00007 #   License: Modified BSD Software License Agreement
00008 #
00009 # $Id: test_shifter.py 1161 2011-03-26 02:10:49Z jack.oquin $
00010 #
00011 
00012 import roslib;
00013 roslib.load_manifest('art_servo')
00014 
00015 import rospy
00016 from art_msgs.msg import Shifter
00017 
00018 def log_shifter_state(state):
00019     rospy.logdebug("shifter gear: %u, relays: %02x (time %.6f)",
00020                    state.gear, state.relays, state.header.stamp.to_sec())
00021 
00022 def log_shifter_cmd(cmd):
00023     #rospy.loginfo(str(cmd))
00024     rospy.loginfo("sending shifter command: " + str(cmd.gear))
00025 
00026 def test():
00027     topic = rospy.Publisher('shifter/cmd', Shifter)
00028     rospy.Subscriber('shifter/state', Shifter, log_shifter_state)
00029     rospy.init_node('test_shifter')
00030 
00031     rospy.loginfo('starting shifter test')
00032 
00033     # initially reset shifter
00034     cmd = Shifter()                     # shifter message
00035     cmd.gear = Shifter.Park
00036     reset_cmd = Shifter()               # reset shifter message
00037     reset_cmd.gear = Shifter.Reset
00038 
00039     while not rospy.is_shutdown():
00040         if cmd.gear > Shifter.Drive:
00041             cmd.gear = Shifter.Park
00042 
00043         # shift to next gear
00044         topic.publish(cmd)
00045         log_shifter_cmd(cmd)
00046         cmd.gear += 1
00047         rospy.sleep(1.0)                # only change once a second
00048 
00049         # reset shifter
00050         topic.publish(reset_cmd)
00051         log_shifter_cmd(reset_cmd)
00052         rospy.sleep(1.0)                # only change once a second
00053 
00054 if __name__ == '__main__':
00055     try:
00056         test()
00057     except rospy.ROSInterruptException: pass
00058 
00059     rospy.loginfo('shifter test completed')


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12