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()