test_brake.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Description:  Unit test for ART brake driver
00004 #
00005 #   Copyright (C) 2009 Austin Robot Technology
00006 #
00007 #   License: Modified BSD Software License Agreement
00008 #
00009 # $Id: test_brake.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 BrakeCommand
00017 from art_msgs.msg import BrakeState
00018 
00019 def log_brake_state(state):
00020     rospy.logdebug("brake position: %.3f  (time %.6f)",
00021                    state.position, state.header.stamp.to_sec())
00022     rospy.logdebug("brake pot, enc, press = (%.3f, %.3f, %.3f)  (time %.6f)",
00023                   state.potentiometer, state.encoder, state.pressure,
00024                   state.header.stamp.to_sec())
00025 
00026 def log_brake_cmd(cmd):
00027     rospy.loginfo("sending brake command: " + str(cmd.request)
00028                   + ", " + str(cmd.position))
00029 
00030 def test():
00031     topic = rospy.Publisher('brake/cmd', BrakeCommand)
00032     rospy.Subscriber('brake/state', BrakeState, log_brake_state)
00033     rospy.init_node('test_brake')
00034 
00035     rospy.loginfo('starting brake test')
00036 
00037     # initially apply full brake
00038     cmd = BrakeCommand()                # brake command msg
00039     cmd.request = BrakeCommand.Absolute
00040     npos = 1.0                          # next position to request
00041     delta = 0.5                         # change requested per cycle
00042 
00043     while not rospy.is_shutdown():
00044 
00045         # check for limits reached
00046         if npos <= 0.0:
00047             npos = 0.0
00048             delta = -delta              # change direction
00049         elif npos >= 1.0:
00050             npos = 1.0
00051             delta = -delta              # change direction
00052 
00053         # format brake command and publish it
00054         cmd.header.stamp = rospy.Time.now()
00055         cmd.position = npos
00056         log_brake_cmd(cmd)
00057         topic.publish(cmd)
00058         npos += delta                   # update next position
00059         rospy.sleep(8.0)
00060 
00061 if __name__ == '__main__':
00062     try:
00063         test()
00064     except rospy.ROSInterruptException: pass
00065 
00066     rospy.loginfo('brake test completed')


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