test_input.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 curses, sys, traceback
00040 
00041 from tf.transformations import euler_from_quaternion
00042 from math import degrees
00043 
00044 from sensor_msgs.msg import Imu
00045 from geometry_msgs.msg import Quaternion
00046 from kobuki_msgs.msg import SensorState
00047 from kobuki_msgs.msg import ButtonEvent
00048 from kobuki_msgs.msg import BumperEvent
00049 from kobuki_msgs.msg import WheelDropEvent
00050 from kobuki_msgs.msg import CliffEvent
00051 from kobuki_msgs.msg import PowerSystemEvent
00052 from kobuki_msgs.msg import DigitalInputEvent
00053 
00054 def ImuCallback(data):
00055   quat = data.orientation
00056   q = [quat.x, quat.y, quat.z, quat.w]
00057   roll, pitch, yaw = euler_from_quaternion(q)
00058   imu_string=str("Gyro Angle: [" + "{0:+.4f}".format(yaw) + " rad]  ["\
00059                  + "{0: >+7.2f}".format(degrees(yaw)) + " deg]"\
00060                  + "            Rate: [" + "{0:+.4f}".format(data.angular_velocity.z) + " rad/s]  ["\
00061                  + "{0: >+7.2f}".format(degrees(data.angular_velocity.z)) + " deg/s] ")
00062   stdscr.addstr(5,3,imu_string)
00063 
00064 def SensorStateCallback(data):
00065   stdscr.addstr(6,3,str("Analog input: [%4d, %4d, %4d, %4d]"     \
00066                                         %(data.analog_input[0], data.analog_input[1], \
00067                                           data.analog_input[2], data.analog_input[3])))
00068 
00069 
00070 digitalS = [False, False, False, False]
00071 
00072 def DigitalInputEventCallback(data):
00073   global digitalS
00074   digitalS = data.values
00075 
00076 button0 = { ButtonEvent.Button0:0, ButtonEvent.Button1:1, ButtonEvent.Button2:2, } 
00077 button1 = { ButtonEvent.RELEASED:'Released', ButtonEvent.PRESSED:'Pressed ', }
00078 buttonS = [ 'Released',  'Released',  'Released', ] 
00079 
00080 def ButtonEventCallback(data):
00081   buttonS[button0[data.button]]=button1[data.state]
00082 
00083 bumper0 = { BumperEvent.LEFT:0, BumperEvent.CENTER:1, BumperEvent.RIGHT:2, } 
00084 bumper1 = { BumperEvent.RELEASED:'Released', BumperEvent.PRESSED:'Pressed ', }
00085 bumperS = [ 'Released',  'Released',  'Released', ] 
00086 
00087 def BumperEventCallback(data):
00088   bumperS[bumper0[data.bumper]]=bumper1[data.state]
00089 
00090 wheel0 = { WheelDropEvent.LEFT:0, WheelDropEvent.RIGHT:1, } 
00091 wheel1 = { WheelDropEvent.RAISED:'Raised ', WheelDropEvent.DROPPED:'Dropped', }
00092 wheelS = [ 'Raised ',  'Raised ', ] 
00093 
00094 def WheelDropEventCallback(data):
00095   wheelS[wheel0[data.wheel]]=wheel1[data.state]
00096 
00097 cliff0 = { CliffEvent.LEFT:0, CliffEvent.CENTER:1, CliffEvent.RIGHT:2, } 
00098 cliff1 = { CliffEvent.FLOOR:'Floor', CliffEvent.CLIFF:'Cliff', }
00099 cliffS = [ 'Floor', 'Floor', 'Floor',]
00100 
00101 def CliffEventCallback(data):
00102   cliffS[cliff0[data.sensor]]=cliff1[data.state]
00103 
00104 power0 = { 
00105   PowerSystemEvent.UNPLUGGED:"Robot unplugged",
00106   PowerSystemEvent.PLUGGED_TO_ADAPTER:"Robot plugged to adapter",
00107   PowerSystemEvent.PLUGGED_TO_DOCKBASE:"Robot plugged to docking base",
00108   PowerSystemEvent.CHARGE_COMPLETED:"Robot charge completed",
00109   PowerSystemEvent.BATTERY_LOW:"Robot battery low",
00110   PowerSystemEvent.BATTERY_CRITICAL:"Robot battery critical", }
00111 
00112 powerS = "Not Available"
00113 
00114 def PowerEventCallback(data):
00115   global powerS
00116   powerS  = power0[data.event]
00117 
00118 def clearing():
00119   curses.echo()
00120   stdscr.keypad(0)
00121   curses.endwin()  
00122 
00123 
00124 if __name__ == '__main__':
00125   stdscr = curses.initscr()
00126   stdscr.addstr(1,1,"Test Every Input of Kobuki")
00127   stdscr.addstr(2,1,"--------------------------")
00128   stdscr.addstr(3,1,"q: Quit")
00129   curses.noecho()
00130   stdscr.keypad(1)
00131   stdscr.nodelay(1)
00132   
00133   rospy.init_node('test_input')
00134   rospy.on_shutdown(clearing)
00135   rospy.Subscriber("/mobile_base/sensors/imu_data", Imu, ImuCallback )
00136   rospy.Subscriber("/mobile_base/sensors/core", SensorState, SensorStateCallback )
00137   rospy.Subscriber("/mobile_base/events/digital_input", DigitalInputEvent, DigitalInputEventCallback )
00138   rospy.Subscriber("/mobile_base/events/button", ButtonEvent, ButtonEventCallback )
00139   rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, BumperEventCallback )
00140   rospy.Subscriber("/mobile_base/events/wheel_drop", WheelDropEvent, WheelDropEventCallback )
00141   rospy.Subscriber("/mobile_base/events/cliff", CliffEvent, CliffEventCallback )
00142   rospy.Subscriber("/mobile_base/events/power_system",PowerSystemEvent,PowerEventCallback)
00143   
00144   try:
00145     while not rospy.is_shutdown():
00146       stdscr.addstr(7,3,str("Digital input: [%5s, %5s, %5s, %5s]"\
00147                        %(str(digitalS[0]), str(digitalS[1]), str(digitalS[2]), str(digitalS[3]))))
00148       stdscr.addstr(8,3,str("Button: B0: %s B1: %s B2: %s"%(buttonS[0], buttonS[1], buttonS[2])))
00149       stdscr.addstr(9,3,str("Bumper: Left: %s Center: %s Right: %s"%(bumperS[0], bumperS[1], bumperS[2])))
00150       stdscr.addstr(10,3,str("WheelDrop: Left: %s Right: %s"%(wheelS[0], wheelS[1])))
00151       stdscr.addstr(11,3,str("Cliff: Left: %s Center: %s Right: %s"%(cliffS[0], cliffS[1], cliffS[2])))
00152       stdscr.addstr(12,3,str("Power: " + "{0: <80s}".format(powerS)))
00153       stdscr.refresh()
00154       key = stdscr.getch()
00155       if key == ord('q'):
00156         rospy.signal_shutdown('user request')
00157     
00158     clearing()
00159   except:
00160     clearing()
00161     traceback.print_exc()  


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