DockDriveControl.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_auto_docking')
00038 import rospy
00039 import sys, select, termios, tty, os
00040 import time
00041 from datetime import datetime
00042 import types
00043 import commands
00044 
00045 from std_msgs.msg import String, Empty
00046 from geometry_msgs.msg import Twist
00047 from kobuki_msgs.msg import DigitalOutput, Led, Sound, SensorState, DockInfraRed, ExternalPower, MotorPower
00048 
00049 
00050 
00051 colorBindings = {
00052     Led.GREEN:Led.ORANGE,
00053     Led.ORANGE:Led.RED,
00054     Led.RED:Led.BLACK,
00055     Led.BLACK:Led.GREEN,
00056 }
00057 
00058 textBindings = {
00059     Led.GREEN:' Green',
00060     Led.ORANGE:'Orange',
00061     Led.RED:'   Red',
00062     Led.BLACK:' Black',
00063 }
00064 
00065 
00066 settings = termios.tcgetattr(sys.stdin.fileno())
00067 
00068 class Controller(object):
00069 
00070   def __init__(self):
00071     #rospy initial setup
00072     rospy.init_node("dock_drive_control")
00073     rospy.on_shutdown(self.clearing)
00074     rate = rospy.Rate(10)
00075     self.message = "Idle"
00076     self.publish_cmd_vel=False
00077     self.cmd_vel=Twist()
00078     self.sensors = SensorState()
00079     self.dock_ir = DockInfraRed()
00080 
00081     self.bat_name = self.getBatteryName()
00082     self.state = "N/A"
00083     self.percentage = 0.0
00084 
00085     self.pub = {
00086 #      'enable':rospy.Publisher('/enable', String),
00087 #      'disable':rospy.Publisher('/disable', String),
00088       'motor_power':rospy.Publisher('/mobile_base/commands/motor_power',MotorPower),
00089 #      'do_dock':rospy.Publisher('/dock_drive/commands/do_dock', Empty),
00090 #      'cancel_dock':rospy.Publisher('/dock_drive/commands/cancel_dock', Empty),
00091       'debug':rospy.Publisher('/dock_drive/debug/mode_shift', String),
00092       'external_power':rospy.Publisher('/mobile_base/commands/external_power',ExternalPower),
00093       'digital_output':rospy.Publisher('/mobile_base/commands/digital_output',DigitalOutput),
00094       'led1':rospy.Publisher('/mobile_base/commands/led1',Led),
00095       'led2':rospy.Publisher('/mobile_base/commands/led2',Led),
00096       'sound':rospy.Publisher('/mobile_base/commands/sound',Sound),
00097       'cmd_vel':rospy.Publisher('/mobile_base/commands/velocity',Twist),
00098     }
00099     self.sub = {
00100       'core':rospy.Subscriber('/mobile_base/sensors/core', SensorState, self.sensorsCallback),
00101       'dock_ir':rospy.Subscriber('/mobile_base/sensors/dock_ir', DockInfraRed, self.dockIRCallback),
00102     }
00103     self.keyBindings = {
00104       '1':(self.pub['debug'].publish, String('enable') ,'enable'),
00105       '2':(self.pub['debug'].publish, String('run') ,'run'),
00106       '3':(self.pub['debug'].publish, String('stop') ,'stop'),
00107       '4':(self.pub['debug'].publish, String('disable'),'disable'),
00108       '5':5,
00109       '6':6,
00110       '7':7,
00111       '8':'eight',
00112       '9':'nine',
00113       '0':'null',
00114 #      'e':(self.pub['motor_power'].publish,MotorPower(MotorPower.ON),'enabled'),
00115 #      'r':(self.pub['motor_power'].publish,MotorPower(MotorPower.OFF),'disabled'),
00116       'e':(self.toggleMotor,True,'enabled'),
00117       'r':(self.toggleMotor,False,'disabled'),
00118       ' ':(self.resetVel,'','resetted'),
00119       'a':(self.pub['sound'].publish,Sound(Sound.ON),'sound.on'),
00120       's':(self.pub['sound'].publish,Sound(Sound.OFF),'sound.off'),
00121       'd':(self.pub['sound'].publish,Sound(Sound.RECHARGE),'sound.recharge'),
00122       'f':(self.pub['sound'].publish,Sound(Sound.BUTTON),'sound.button'),
00123       'z':(self.pub['sound'].publish,Sound(Sound.ERROR),'sound.error'),
00124       'x':(self.pub['sound'].publish,Sound(Sound.CLEANINGSTART),'sound.cleaningstart'),
00125       'c':(self.pub['sound'].publish,Sound(Sound.CLEANINGEND),'sound.cleaningend'),
00126       'q':(rospy.signal_shutdown,'user reuest','quit'),
00127       'Q':(rospy.signal_shutdown,'user reuest','quit'),
00128     }
00129     rospy.Timer(rospy.Duration(0.1), self.keyopCallback)
00130     if len(self.bat_name) > 0:
00131       rospy.Timer(rospy.Duration(1.0), self.batteryCallback)
00132     rospy.Timer(rospy.Duration(1.0), self.stateCallback) # to check status of rostopics
00133     self.printFront()
00134     '''
00135     # initial values
00136     external_power = DigitalOutput()
00137     external_power.values = [ True, True, True, True ]
00138     external_power.mask = [ True, True, True, True ]
00139     
00140     digital_output = DigitalOutput()
00141     digital_output.values = [ True, True, True, True ]
00142     digital_output.mask = [ True, True, True, True ]
00143     
00144     leds = []
00145     leds.append(Led())
00146     leds.append(Led())
00147     leds[0].value = Led.GREEN
00148     leds[1].value = Led.GREEN
00149     
00150     # initialize outputs
00151     while not pub_ext_pwr.get_num_connections():
00152       rate.sleep()
00153     pub_ext_pwr.publish(external_power)
00154     
00155     while not pub_dgt_out.get_num_connections():
00156       rate.sleep()
00157     pub_dgt_out.publish(digital_output)
00158     
00159     while not pub[0].get_num_connections():
00160       rate.sleep()
00161     pub[0].publish(leds[0])
00162     
00163     while not pub[1].get_num_connections():
00164       rate.sleep()
00165     pub[1].publish(leds[1])
00166     '''
00167 
00168   def spin(self):
00169     while not rospy.is_shutdown():
00170       self.printStatus() 
00171       self.processKeys()
00172 
00173   def getKey(self):
00174     fd = sys.stdin.fileno()
00175     new = termios.tcgetattr(fd)
00176     new[3] = new[3] & ~termios.ICANON & ~termios.ECHO
00177     new[6][termios.VMIN] = 0
00178     new[6][termios.VTIME] = 1
00179     termios.tcsetattr(fd, termios.TCSANOW, new)
00180     return sys.stdin.read(1)
00181     #return os.read(fd,1)  #equivalent
00182     #return key
00183 
00184   def processKeys(self):
00185     key = self.getKey()
00186     if key == '':
00187       #self.message = "non." #debug
00188       return
00189     if ord(key) == 27:
00190       akey = self.getKey()
00191       if len(akey) and ord(akey)==91:
00192         key = self.getKey()
00193         if self.publish_cmd_vel:
00194           if key == 'A': self.message = 'Up Arrow'    ; self.cmd_vel.linear.x += 0.05
00195           if key == 'B': self.message = 'Down Arrow'  ; self.cmd_vel.linear.x -= 0.05
00196           if key == 'C': self.message = 'Right Arrow' ; self.cmd_vel.angular.z -= 0.33
00197           if key == 'D': self.message = 'Left Arrow'  ; self.cmd_vel.angular.z += 0.33
00198         return
00199     if key in self.keyBindings.keys():
00200       if type(self.keyBindings[key]) is types.TupleType:
00201         f = self.keyBindings[key][0]
00202         v = self.keyBindings[key][1]
00203         m = self.keyBindings[key][2]
00204         if type(f) is types.InstanceType or type(f) is types.MethodType or type(f) is types.FunctionType : f(v)
00205         else:
00206           print type(f)
00207         self.message = str(m)
00208       else:
00209         self.message = str(self.keyBindings[key])
00210     else:
00211       self.message = "unknown key: " + str(len(key)) + ": " + str(ord(key))
00212     return
00213     '''
00214     elif key in keyBindings1.keys():
00215         external_power.values[keyBindings1[key]] ^= True
00216         printStatus(external_power.values, digital_output.values, leds)
00217         pub_ext_pwr.publish(external_power)
00218 
00219     elif key in keyBindings3.keys():
00220         digital_output.values[keyBindings3[key]] ^= True
00221         printStatus(external_power.values, digital_output.values, leds)
00222         pub_dgt_out.publish(digital_output)
00223 
00224     elif key in keyBindings2.keys():
00225         leds[keyBindings2[key]].value = colorBindings[ leds[keyBindings2[key]].value ]
00226         printStatus(external_power.values, digital_output.values, leds)
00227         pub[keyBindings2[key]].publish(leds[keyBindings2[key]])
00228     elif key in keyBindings4.keys():
00229         pub_sounds.publish(keyBindings4[key])
00230         #rate.sleep()
00231     '''
00232 
00233   def clearing(self):
00234     sys.stdout.write('\n\r')
00235     sys.stdout.flush()
00236     termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00237 
00238   def toggleMotor(self, on_off):
00239     if on_off:
00240       self.pub['motor_power'].publish(MotorPower(MotorPower.ON))
00241       self.publish_cmd_vel = True
00242     else:
00243       self.pub['motor_power'].publish(MotorPower(MotorPower.OFF))
00244       self.publish_cmd_vel = False
00245       self.cmd_vel = Twist()
00246 
00247   def keyopCallback(self, event):
00248     if not rospy.is_shutdown():
00249       if self.publish_cmd_vel:
00250         self.pub['cmd_vel'].publish(self.cmd_vel)
00251 
00252   def sensorsCallback(self, data):
00253     if not rospy.is_shutdown():
00254       self.sensors = data
00255 
00256   def dockIRCallback(self, data):
00257     if not rospy.is_shutdown():
00258       self.dock_ir = data
00259 
00260   def batteryCallback(self, event):
00261     rem = float(commands.getoutput("grep \"^remaining capacity\" /proc/acpi/battery/" + self.bat_name + "/state | awk '{ print $3 }'"))
00262     full = float(commands.getoutput("grep \"^last full capacity\" /proc/acpi/battery/" + self.bat_name + "/info | awk '{ print $4 }'"))
00263     self.state = commands.getoutput("grep \"^charging state\" /proc/acpi/battery/" + self.bat_name + "/state | awk '{ print $3 }'")
00264     self.percentage = float((rem/full) * 100.)
00265 
00266   def stateCallback(self, event):
00267     # do check
00268     num =  self.sub['core'].get_num_connections()
00269     if num  == 0:
00270       self.messages = 'core is disconnected({0:d})'.format(num)
00271     else:
00272       self.messages = 'core is connected({0:d})'.format(num)
00273     # do more handling and managing
00274     # get_num_connections are not reliable yet.
00275 
00276   def resetVel(self, x):
00277     self.cmd_vel = Twist()
00278 
00279   def printFront(self): 
00280     # statement
00281     print ""
00282     print " Dock Drive Controller"
00283     print "----------------------"
00284     print "1: do_dock"
00285     print "2: run"
00286     print "3: stop" 
00287     print "4: cancel_dock"
00288     print ""
00289     print "q: quit"
00290     print ""
00291     #print "  3.3V  5.0V 12V5A 12V1A |   Led #1   Led #2 |  DO_0  DO_1  DO_2  DO_3"
00292     #print " [ On] [Off] [ On] [Off] | [Orange] [Orange] | [ On] [Off] [Off] [ On]"
00293 
00294   def printStatus(self):
00295     sys.stdout.write('                                                                                                               \r')
00296     sys.stdout.write('[ \033[1m' + self.message + '\033[0m ]')
00297 
00298     if len(self.bat_name) > 0:
00299       sys.stdout.write('[Laptop: ' + "{0:2.2f}".format(self.percentage) + '% - ' + self.state + ']')
00300 
00301     src_str = ''
00302     if self.sensors.charger:
00303       src_str = '(adaptor)' if self.sensors.charger&16 else '(dock)'
00304 
00305     chg = self.sensors.charger&6
00306     if chg==0: chg_str = 'discharging'
00307     elif chg==2: chg_str = 'fully charged'
00308     else: chg_str = 'charging'
00309     sys.stdout.write('[Robot: ' + "{0:2.1f}".format(self.sensors.battery/10.) + 'V - ' + chg_str + src_str + ']')
00310     '''
00311     for idx in range(0,4):
00312         if ext_values[idx]:
00313             sys.stdout.write('[ \033[1mOn\033[0m] ')
00314         else: 
00315             sys.stdout.write('[Off] ')
00316     sys.stdout.write('| [\033[1m'+textBindings[leds[0].value]+'\033[0m] ')
00317     sys.stdout.write('[\033[1m'+textBindings[leds[1].value]+'\033[0m] | ')
00318 
00319     for idx in range(0,4):
00320         if dgt_values[idx]:
00321             sys.stdout.write('[ \033[1mOn\033[0m] ')
00322         else: 
00323             sys.stdout.write('[Off] ')
00324     '''
00325     sys.stdout.write('\r')
00326     sys.stdout.flush()
00327 
00328   def getBatteryName(self):
00329     PATH = '/proc/acpi/battery'
00330     bat_name = ''
00331     if os.path.exists(PATH):
00332       bat = os.walk(PATH).next()
00333       if len(bat[1]) > 0:
00334         bat_name = bat[1][0]
00335     return bat_name
00336 
00337 
00338 if __name__ == '__main__':
00339   try:
00340     instance = Controller()
00341     instance.spin()
00342   except rospy.ROSInterruptException: pass


kobuki_auto_docking
Author(s): Younghun Ju
autogenerated on Thu Jun 6 2019 17:37:33