test_output.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2012, Yujin Robot
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Yujin Robot nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 # Author: Younghun Ju <yhju@yujinrobot.com> <yhju83@gmail.com>
00036 
00037 import roslib; roslib.load_manifest('kobuki_testsuite')
00038 import rospy
00039 import sys, select, termios, tty
00040 import time
00041 from datetime import datetime
00042 
00043 from kobuki_msgs.msg import DigitalOutput, Led, Sound
00044 
00045 
00046 def getKey():
00047     tty.setraw(sys.stdin.fileno())
00048     rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
00049     if rlist:
00050         key = sys.stdin.read(1)
00051     else:
00052         key = ''
00053 
00054     #termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00055     return key
00056 
00057 def printStatus(ext_values, dgt_values, leds):
00058     sys.stdout.write('\r ')
00059     for idx in range(0,4):
00060         if ext_values[idx]:
00061             sys.stdout.write('[ \033[1mOn\033[0m] ')
00062         else: 
00063             sys.stdout.write('[Off] ')
00064     sys.stdout.write('| [\033[1m'+textBindings[leds[0].value]+'\033[0m] ')
00065     sys.stdout.write('[\033[1m'+textBindings[leds[1].value]+'\033[0m] | ')
00066 
00067     for idx in range(0,4):
00068         if dgt_values[idx]:
00069             sys.stdout.write('[ \033[1mOn\033[0m] ')
00070         else: 
00071             sys.stdout.write('[Off] ')
00072 
00073     sys.stdout.write('\r')
00074     sys.stdout.flush()
00075 
00076 keyBindings1 = {
00077     '1':0,
00078     '2':1,
00079     '3':2,
00080     '4':3,
00081 }
00082 
00083 keyBindings2 = {
00084     '5':0,
00085     '6':1,
00086 }
00087 
00088 keyBindings3 = {
00089     '7':0,
00090     '8':1,
00091     '9':2,
00092     '0':3,
00093 }
00094 
00095 keyBindings4 = {
00096     'a':Sound.ON,
00097     's':Sound.OFF,
00098     'd':Sound.RECHARGE,
00099     'f':Sound.BUTTON,
00100     'z':Sound.ERROR,
00101     'x':Sound.CLEANINGSTART,
00102     'c':Sound.CLEANINGEND,
00103 }
00104 
00105 colorBindings = {
00106     Led.GREEN:Led.ORANGE,
00107     Led.ORANGE:Led.RED,
00108     Led.RED:Led.BLACK,
00109     Led.BLACK:Led.GREEN,
00110 }
00111 
00112 textBindings = {
00113     Led.GREEN:' Green',
00114     Led.ORANGE:'Orange',
00115     Led.RED:'   Red',
00116     Led.BLACK:' Black',
00117 }
00118 
00119 
00120 settings = termios.tcgetattr(sys.stdin)
00121 
00122 def clearing():
00123   sys.stdout.write('\n\r')
00124   sys.stdout.flush()
00125   termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00126   
00127 #rospy initial setup
00128 rospy.init_node("test_output")
00129 rospy.on_shutdown(clearing)
00130 rate = rospy.Rate(10)
00131 
00132 pub_ext_pwr = rospy.Publisher('/mobile_base/commands/external_power',DigitalOutput)
00133 pub_dgt_out = rospy.Publisher('/mobile_base/commands/digital_output',DigitalOutput)
00134 pub = []
00135 pub.append(rospy.Publisher('/mobile_base/commands/led1',Led))
00136 pub.append(rospy.Publisher('/mobile_base/commands/led2',Led))
00137 pub_sounds = rospy.Publisher('/mobile_base/commands/sound',Sound)
00138 
00139 # initial values
00140 external_power = DigitalOutput()
00141 external_power.values = [ True, True, True, True ]
00142 external_power.mask = [ True, True, True, True ]
00143 
00144 digital_output = DigitalOutput()
00145 digital_output.values = [ True, True, True, True ]
00146 digital_output.mask = [ True, True, True, True ]
00147 
00148 leds = []
00149 leds.append(Led())
00150 leds.append(Led())
00151 leds[0].value = Led.GREEN
00152 leds[1].value = Led.GREEN
00153 
00154 # initialize outputs
00155 while not pub_ext_pwr.get_num_connections():
00156   rate.sleep()
00157 pub_ext_pwr.publish(external_power)
00158 
00159 while not pub_dgt_out.get_num_connections():
00160   rate.sleep()
00161 pub_dgt_out.publish(digital_output)
00162 
00163 while not pub[0].get_num_connections():
00164   rate.sleep()
00165 pub[0].publish(leds[0])
00166 
00167 while not pub[1].get_num_connections():
00168   rate.sleep()
00169 pub[1].publish(leds[1])
00170 
00171 # statement
00172 print ""
00173 print "Control Every Output of Kobuki"
00174 print "------------------------------"
00175 print "1: Toggle the state of 3.3V"
00176 print "2: Toggle the state of 5V"
00177 print "3: Toggle the state of 12V5A(arm)"
00178 print "4: Toggle the state of 12V1A(kinect)"
00179 print ""
00180 print "5: Control Led #1"
00181 print "6: Control Led #2"
00182 print ""
00183 print "7~0: Toggle the state of DigitalOut_0~3"
00184 print ""
00185 print "Play Sounds"
00186 print "-----------"
00187 print "a: On   s: Off   d: Recharge   f: Button   z: Error   x: CleaningStart   c: CleaningEnd"
00188 print ""
00189 print "q: Quit"
00190 print ""
00191 print ""
00192 print "  3.3V  5.0V 12V5A 12V1A |   Led #1   Led #2 |  DO_0  DO_1  DO_2  DO_3"
00193 #print " [ On] [Off] [ On] [Off] | [Orange] [Orange] | [ On] [Off] [Off] [ On]"
00194 
00195 while not rospy.is_shutdown():
00196     printStatus(external_power.values, digital_output.values, leds)
00197     key = getKey()
00198     if key == '': continue
00199     if key == 'q' or key == 'Q': 
00200        rospy.signal_shutdown('user reuest')
00201 
00202     elif key in keyBindings1.keys():
00203         external_power.values[keyBindings1[key]] ^= True
00204         printStatus(external_power.values, digital_output.values, leds)
00205         pub_ext_pwr.publish(external_power)
00206 
00207     elif key in keyBindings3.keys():
00208         digital_output.values[keyBindings3[key]] ^= True
00209         printStatus(external_power.values, digital_output.values, leds)
00210         pub_dgt_out.publish(digital_output)
00211 
00212     elif key in keyBindings2.keys():
00213         leds[keyBindings2[key]].value = colorBindings[ leds[keyBindings2[key]].value ]
00214         printStatus(external_power.values, digital_output.values, leds)
00215         pub[keyBindings2[key]].publish(leds[keyBindings2[key]])
00216     elif key in keyBindings4.keys():
00217         pub_sounds.publish(keyBindings4[key])
00218         #rate.sleep()
00219 


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Wed Sep 16 2015 04:35:48