asr_vosk: Allow to handle keyword and command in one sentence
[ros_wild_thumper.git] / scripts / self_test.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import subprocess
5 import re
6 import rospy
7 import tf
8 from time import sleep
9 from math import *
10 from geometry_msgs.msg import Twist
11 from sensor_msgs.msg import Imu, Range, BatteryState
12 from wild_thumper.msg import LedStripe, Led
13
14 class SelfTest:
15         def __init__(self):
16                 rospy.init_node('wild_thumper_self_test')
17                 self.voltage = None
18                 self.current = None
19                 self.yaw = None
20                 self.pub_led = rospy.Publisher("led_stripe", LedStripe, queue_size=16)
21                 self.pub_vel = rospy.Publisher("cmd_vel_out", Twist, queue_size=16)
22                 self.sub_bat = rospy.Subscriber("battery", BatteryState, self.batteryReceived)
23                 rospy.Subscriber("imu", Imu, self.imuReceived)
24                 rospy.Subscriber("range_forward_left", Range, self.rangeReceived)
25                 rospy.Subscriber("range_forward_right", Range, self.rangeReceived)
26                 rospy.Subscriber("range_backward", Range, self.rangeReceived)
27                 rospy.Subscriber("range_left", Range, self.rangeReceived)
28                 rospy.Subscriber("range_right", Range, self.rangeReceived)
29                 self.run()
30
31         def run(self):
32                 self.test_led()
33                 self.test_power()
34                 self.test_wlan()
35                 self.test_engine()
36                 self.test_range()
37
38         def test_range(self):
39                 assert self.sonar_forward_left > 0.04 and self.sonar_forward_left < 6
40                 assert self.sonar_forward_right > 0.04 and self.sonar_forward_right < 6
41                 assert self.sonar_backward > 0.04 and self.sonar_backward < 6
42
43         def rangeReceived(self, msg):
44                 if msg.header.frame_id == "sonar_forward_left":
45                         self.sonar_forward_left = msg.range
46                 elif msg.header.frame_id == "sonar_forward_right":
47                         self.sonar_forward_right = msg.range
48                 elif msg.header.frame_id == "sonar_backward":
49                         self.sonar_backward = msg.range
50                 else:
51                         raise Exception("Unknown frame %s" % msg.header.frame_id)
52
53         def test_engine(self):
54                 msg = Twist()
55
56                 # forward
57                 msg.linear.x = 0.20
58                 msg.angular.z = 0
59                 self.pub_vel.publish(msg)
60                 sleep(1)
61                 msg.linear.x = 0
62                 msg.angular.z = 0
63                 self.pub_vel.publish(msg)
64
65                 # turn
66                 yaw1 = self.yaw
67                 msg.linear.x = 0
68                 msg.angular.z = pi/2
69                 self.pub_vel.publish(msg)
70                 sleep(1)
71                 msg.linear.x = 0
72                 msg.angular.z = 0
73                 self.pub_vel.publish(msg)
74                 yaw2 = self.yaw
75                 diff = abs(pi/2 - (yaw2 - yaw1))
76                 if diff > 2*pi:
77                         diff-=2*pi
78                 assert diff < pi/4
79
80         def imuReceived(self, msg):
81                 (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
82                 self.yaw = yaw
83
84         def test_wlan(self):
85                 cmd = subprocess.Popen('/sbin/iwconfig wlan0', shell=True, stdout=subprocess.PIPE)
86                 for line in cmd.stdout:
87                         if "Bit Rate" in line:
88                                 bit_rate = float(re.findall(r"[\w]+", line.strip())[2])
89                                 assert bit_rate > 50
90
91         def test_power(self):
92                 assert self.voltage > 6 and self.voltage <= 12
93                 assert self.current > 0.2 and self.current < 1
94
95         def batteryReceived(self, msg):
96                 self.voltage = msg.voltage
97                 self.current = msg.current
98                 self.sub_bat.unregister()
99
100         def test_led(self):
101                 led_msg = LedStripe()
102                 led_msg = [Led()]
103                 for i in range(16):
104                         led_msg[0].num = i
105                         led_msg[0].red = 10
106                         led_msg[0].green = 0
107                         led_msg[0].blue = 0
108                         self.pub_led.publish(led_msg)
109                         sleep(0.1)
110                 for i in range(16):
111                         led_msg[0].num = i
112                         led_msg[0].red = 0
113                         led_msg[0].green = 10
114                         led_msg[0].blue = 0
115                         self.pub_led.publish(led_msg)
116                         sleep(0.1)
117                 for i in range(16):
118                         led_msg[0].num = i
119                         led_msg[0].red = 0
120                         led_msg[0].green = 0
121                         led_msg[0].blue = 10
122                         self.pub_led.publish(led_msg)
123                         sleep(0.1)
124                 for i in range(16):
125                         led_msg[0].num = i
126                         led_msg[0].red = 0
127                         led_msg[0].green = 0
128                         led_msg[0].blue = 0
129                         self.pub_led.publish(led_msg)
130                         sleep(0.1)
131
132 if __name__ == "__main__":
133         SelfTest()