relays.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Set relays using ART ioadr interface.
00004 #
00005 #   Copyright (C) 2009 Austin Robot Technology
00006 #
00007 #   License: Modified BSD Software License Agreement
00008 #
00009 # $Id: relays.py 865 2010-11-26 19:49:19Z jack.oquin $
00010 #
00011 
00012 # make print compatible with python 2.6 and 3.0
00013 from __future__ import print_function
00014 
00015 import roslib;
00016 roslib.load_manifest('art_servo')
00017 
00018 import sys
00019 import string
00020 import rospy
00021 from art_msgs.msg import IOadrCommand
00022 from art_msgs.msg import IOadrState
00023 
00024 # global variables
00025 
00026 cur_relays = None
00027 started = False
00028 finished = False
00029 
00030 def ioadr_state_update(state):
00031     "ioadr/state message callback"
00032     global cur_relays, started, finished
00033     if started:
00034         if cur_relays != state.relays:
00035             rospy.loginfo("relays changed from 0x%02x to 0x%02x",
00036                           cur_relays, state.relays)
00037             finished = True
00038     cur_relays = state.relays
00039     started = True
00040 
00041 def relays(relays_on, relays_off = 0):
00042     "set and clear IOADR8x relays"
00043     rospy.init_node('relays')
00044     topic = rospy.Publisher('ioadr/cmd', IOadrCommand)
00045     rospy.Subscriber('ioadr/state', IOadrState, ioadr_state_update)
00046 
00047     rospy.loginfo('waiting for relays from ioadr driver')
00048     global started
00049     while not started:
00050         rospy.sleep(0.1)        # wait for relay state input
00051         if rospy.is_shutdown():
00052             rospy.logwarn('command interrupted, node shut down')
00053             return 
00054 
00055     # TODO check if no change required, exit if so
00056     global cur_relays
00057     old_relays = cur_relays
00058     if ((old_relays & ~relays_off) | relays_on) == old_relays:
00059         rospy.loginfo('nothing to do, relays already 0x%02x', old_relays)
00060         return
00061 
00062     cmd = IOadrCommand()        # ioadr/cmd message
00063     cmd.relays_on = relays_on
00064     cmd.relays_off = relays_off
00065     rospy.loginfo('sending command now')
00066 
00067     global finished
00068     while not finished and not rospy.is_shutdown():
00069         topic.publish(cmd)
00070         rospy.sleep(0.5)        # wait for relays to change
00071 
00072     rospy.loginfo('finished setting relays')
00073 
00074 def usage():
00075     "print usage message"
00076     print("""
00077 Usage:
00078 
00079     rosrun art_servo relays.py <set> [<clear>]
00080 
00081 Where <set> and <clear> are hex relay values, such as 0x80 or 4.
00082 
00083 Not implemented:
00084 
00085   Allow <set> and <clear> to be relay names such as: ENABLED, RUN,
00086   FLASHER, ALARM, LASER_FRONT, LASER_TOP, TURN_LEFT, or TURN_RIGHT.
00087   These names are not case-sensitive.
00088 
00089 """)
00090 
00091 if __name__ == '__main__':
00092 
00093     if len(sys.argv) < 2:
00094         usage()
00095         exit(1)
00096 
00097     #gears = {'park':    IOadrState.Park,
00098     #         'reverse': IOadrState.Reverse,
00099     #         'neutral': IOadrState.Neutral,
00100     #         'drive':   IOadrState.Drive}
00101     #
00102     #expand_abbrev = {'p': 'park',
00103     #                 'r': 'reverse',
00104     #                 'n': 'neutral',
00105     #                 'd': 'drive'}
00106     #
00107     #gear_name = string.lower(sys.argv[1])
00108     #
00109     ## expand abbreviated names
00110     #if gear_name in expand_abbrev:
00111     #    gear_name = expand_abbrev[gear_name]
00112     #
00113     #if not gear_name in gears:
00114     #    print('unknown gear:', gear_name)
00115     #    usage()
00116     #    exit(2)
00117     #
00118     #target_gear = gears[gear_name]
00119 
00120     relays_on = int(sys.argv[1], 16)
00121     relays_off = 0
00122     if len(sys.argv) > 2:
00123         relays_off = int(sys.argv[2], 16)
00124     rospy.loginfo('setting relays on: ' + hex(relays_on)
00125                   + ', off: ' + hex(relays_off))
00126 
00127     try:
00128         relays(relays_on, relays_off)
00129     except rospy.ROSInterruptException: pass
00130 
00131     exit(0)


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