test_input.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2012, Yujin Robot
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of the Yujin Robot nor the names of its
19 # contributors may be used to endorse or promote products derived
20 # from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 
35 # Author: Younghun Ju <yhju@yujinrobot.com> <yhju83@gmail.com>
36 
37 import roslib; roslib.load_manifest('kobuki_testsuite')
38 import rospy
39 import curses, sys, traceback
40 
41 from tf.transformations import euler_from_quaternion
42 from math import degrees
43 
44 from sensor_msgs.msg import Imu
45 from geometry_msgs.msg import Quaternion
46 from kobuki_msgs.msg import SensorState
47 from kobuki_msgs.msg import ButtonEvent
48 from kobuki_msgs.msg import BumperEvent
49 from kobuki_msgs.msg import WheelDropEvent
50 from kobuki_msgs.msg import CliffEvent
51 from kobuki_msgs.msg import PowerSystemEvent
52 from kobuki_msgs.msg import DigitalInputEvent
53 
54 def ImuCallback(data):
55  quat = data.orientation
56  q = [quat.x, quat.y, quat.z, quat.w]
57  roll, pitch, yaw = euler_from_quaternion(q)
58  imu_string=str("Gyro Angle: [" + "{0:+.4f}".format(yaw) + " rad] ["\
59  + "{0: >+7.2f}".format(degrees(yaw)) + " deg]"\
60  + " Rate: [" + "{0:+.4f}".format(data.angular_velocity.z) + " rad/s] ["\
61  + "{0: >+7.2f}".format(degrees(data.angular_velocity.z)) + " deg/s] ")
62  stdscr.addstr(5,3,imu_string)
63 
65  stdscr.addstr(6,3,str("Analog input: [%4d, %4d, %4d, %4d]" \
66  %(data.analog_input[0], data.analog_input[1], \
67  data.analog_input[2], data.analog_input[3])))
68 
69 
70 digitalS = [False, False, False, False]
71 
73  global digitalS
74  digitalS = data.values
75 
76 button0 = { ButtonEvent.Button0:0, ButtonEvent.Button1:1, ButtonEvent.Button2:2, }
77 button1 = { ButtonEvent.RELEASED:'Released', ButtonEvent.PRESSED:'Pressed ', }
78 buttonS = [ 'Released', 'Released', 'Released', ]
79 
81  buttonS[button0[data.button]]=button1[data.state]
82 
83 bumper0 = { BumperEvent.LEFT:0, BumperEvent.CENTER:1, BumperEvent.RIGHT:2, }
84 bumper1 = { BumperEvent.RELEASED:'Released', BumperEvent.PRESSED:'Pressed ', }
85 bumperS = [ 'Released', 'Released', 'Released', ]
86 
88  bumperS[bumper0[data.bumper]]=bumper1[data.state]
89 
90 wheel0 = { WheelDropEvent.LEFT:0, WheelDropEvent.RIGHT:1, }
91 wheel1 = { WheelDropEvent.RAISED:'Raised ', WheelDropEvent.DROPPED:'Dropped', }
92 wheelS = [ 'Raised ', 'Raised ', ]
93 
95  wheelS[wheel0[data.wheel]]=wheel1[data.state]
96 
97 cliff0 = { CliffEvent.LEFT:0, CliffEvent.CENTER:1, CliffEvent.RIGHT:2, }
98 cliff1 = { CliffEvent.FLOOR:'Floor', CliffEvent.CLIFF:'Cliff', }
99 cliffS = [ 'Floor', 'Floor', 'Floor',]
100 
102  cliffS[cliff0[data.sensor]]=cliff1[data.state]
103 
104 power0 = {
105  PowerSystemEvent.UNPLUGGED:"Robot unplugged",
106  PowerSystemEvent.PLUGGED_TO_ADAPTER:"Robot plugged to adapter",
107  PowerSystemEvent.PLUGGED_TO_DOCKBASE:"Robot plugged to docking base",
108  PowerSystemEvent.CHARGE_COMPLETED:"Robot charge completed",
109  PowerSystemEvent.BATTERY_LOW:"Robot battery low",
110  PowerSystemEvent.BATTERY_CRITICAL:"Robot battery critical", }
111 
112 powerS = "Not Available"
113 
115  global powerS
116  powerS = power0[data.event]
117 
118 def clearing():
119  curses.echo()
120  stdscr.keypad(0)
121  curses.endwin()
122 
123 
124 if __name__ == '__main__':
125  stdscr = curses.initscr()
126  stdscr.addstr(1,1,"Test Every Input of Kobuki")
127  stdscr.addstr(2,1,"--------------------------")
128  stdscr.addstr(3,1,"q: Quit")
129  curses.noecho()
130  stdscr.keypad(1)
131  stdscr.nodelay(1)
132 
133  rospy.init_node('test_input')
134  rospy.on_shutdown(clearing)
135  rospy.Subscriber("/mobile_base/sensors/imu_data", Imu, ImuCallback )
136  rospy.Subscriber("/mobile_base/sensors/core", SensorState, SensorStateCallback )
137  rospy.Subscriber("/mobile_base/events/digital_input", DigitalInputEvent, DigitalInputEventCallback )
138  rospy.Subscriber("/mobile_base/events/button", ButtonEvent, ButtonEventCallback )
139  rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, BumperEventCallback )
140  rospy.Subscriber("/mobile_base/events/wheel_drop", WheelDropEvent, WheelDropEventCallback )
141  rospy.Subscriber("/mobile_base/events/cliff", CliffEvent, CliffEventCallback )
142  rospy.Subscriber("/mobile_base/events/power_system",PowerSystemEvent,PowerEventCallback)
143 
144  try:
145  while not rospy.is_shutdown():
146  stdscr.addstr(7,3,str("Digital input: [%5s, %5s, %5s, %5s]"\
147  %(str(digitalS[0]), str(digitalS[1]), str(digitalS[2]), str(digitalS[3]))))
148  stdscr.addstr(8,3,str("Button: B0: %s B1: %s B2: %s"%(buttonS[0], buttonS[1], buttonS[2])))
149  stdscr.addstr(9,3,str("Bumper: Left: %s Center: %s Right: %s"%(bumperS[0], bumperS[1], bumperS[2])))
150  stdscr.addstr(10,3,str("WheelDrop: Left: %s Right: %s"%(wheelS[0], wheelS[1])))
151  stdscr.addstr(11,3,str("Cliff: Left: %s Center: %s Right: %s"%(cliffS[0], cliffS[1], cliffS[2])))
152  stdscr.addstr(12,3,str("Power: " + "{0: <80s}".format(powerS)))
153  stdscr.refresh()
154  key = stdscr.getch()
155  if key == ord('q'):
156  rospy.signal_shutdown('user request')
157 
158  clearing()
159  except:
160  clearing()
161  traceback.print_exc()
def WheelDropEventCallback(data)
Definition: test_input.py:94
def PowerEventCallback(data)
Definition: test_input.py:114
def BumperEventCallback(data)
Definition: test_input.py:87
def ButtonEventCallback(data)
Definition: test_input.py:80
def DigitalInputEventCallback(data)
Definition: test_input.py:72
def SensorStateCallback(data)
Definition: test_input.py:64
def CliffEventCallback(data)
Definition: test_input.py:101
def clearing()
Definition: test_input.py:118
def ImuCallback(data)
Definition: test_input.py:54


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Mon Jun 10 2019 13:45:22