2 # -*- coding: iso-8859-15 -*-
10 from geometry_msgs.msg import Twist
11 from sensor_msgs.msg import Imu, Range, BatteryState
12 from wild_thumper.msg import LedStripe, Led
16 rospy.init_node('wild_thumper_self_test')
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)
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
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
51 raise Exception("Unknown frame %s" % msg.header.frame_id)
53 def test_engine(self):
59 self.pub_vel.publish(msg)
63 self.pub_vel.publish(msg)
69 self.pub_vel.publish(msg)
73 self.pub_vel.publish(msg)
75 diff = abs(pi/2 - (yaw2 - yaw1))
80 def imuReceived(self, msg):
81 (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
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])
92 assert self.voltage > 6 and self.voltage <= 12
93 assert self.current > 0.2 and self.current < 1
95 def batteryReceived(self, msg):
96 self.voltage = msg.voltage
97 self.current = msg.current
98 self.sub_bat.unregister()
101 led_msg = LedStripe()
108 self.pub_led.publish(led_msg)
113 led_msg[0].green = 10
115 self.pub_led.publish(led_msg)
122 self.pub_led.publish(led_msg)
129 self.pub_led.publish(led_msg)
132 if __name__ == "__main__":