add self_test script
[ros_wild_thumper.git] / scripts / tinkerforge_imu2.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import rospy
5 import prctl
6 from math import *
7 from sensor_msgs.msg import Imu
8 from tinkerforge.ip_connection import IPConnection
9 from tinkerforge.brick_imu_v2 import BrickIMUV2
10
11 class ImuBrickv2:
12         def __init__(self):
13                 rospy.init_node('imu_brick_v2')
14                 prctl.set_name("imu_brick_v2")
15                 self.pub_imu = rospy.Publisher("imu", Imu, queue_size=16)
16
17                 host = rospy.get_param('~host', 'localhost')
18                 port = rospy.get_param('~port', 4223)
19                 uid = rospy.get_param('~uid')
20
21                 ipcon = IPConnection() # Create IP connection
22                 imu = BrickIMUV2(uid, ipcon) # Create device object
23                 ipcon.connect(host, port) # Connect to brickd
24
25                 imu.leds_off()
26                 #imu.disable_status_led()
27
28                 imu.register_callback(imu.CALLBACK_ALL_DATA, self.cb_all_data)
29                 imu.set_all_data_period(20) # ms
30
31                 rospy.spin()
32                 ipcon.disconnect()
33
34
35         def cb_all_data(self, acceleration, magnetic_field, angular_velocity, euler_angle, quaternion, linear_acceleration, gravity_vector, temperature, calibration_status):
36                 msg = Imu()
37                 msg.header.stamp = rospy.Time.now()
38                 msg.header.frame_id = "base_imu_link"
39
40                 msg.orientation.x = quaternion[1]/16383.0
41                 msg.orientation.y = quaternion[2]/16383.0
42                 msg.orientation.z = quaternion[3]/16383.0
43                 msg.orientation.w = quaternion[0]/16383.0
44                 # Observed orientation variance: 0.0 (10k samples)
45                 # Magnometer heading accuracy is +-2.5 deg => 0.088 rad
46                 # With heading accuracy as std dev, variance = 0.088^2 = 0.008 
47                 msg.orientation_covariance = [
48                         0.008, 0     , 0,
49                         0    , 0.008, 0,
50                         0    , 0    , 0.008
51                 ]
52
53                 msg.angular_velocity.x = angular_velocity[0]/16.0*pi/180
54                 msg.angular_velocity.y = angular_velocity[1]/16.0*pi/180
55                 msg.angular_velocity.z = angular_velocity[2]/16.0*pi/180
56                 # Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
57                 msg.angular_velocity_covariance = [
58                         0.02, 0   , 0,
59                         0   , 0.02, 0,
60                         0   , 0   , 0.02
61                 ]
62
63                 msg.linear_acceleration.x = acceleration[0]/100.0
64                 msg.linear_acceleration.y = acceleration[1]/100.0
65                 msg.linear_acceleration.z = acceleration[2]/100.0
66                 # Observed linear acceleration variance: 0.001532 (10k samples)
67                 # Calculation for variance taken from razor imu:
68                 # nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2
69                 # Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
70                 msg.linear_acceleration_covariance = [
71                         0.04, 0    , 0,
72                         0    , 0.04, 0,
73                         0    , 0    , 0.04
74                 ]
75
76                 self.pub_imu.publish(msg)
77
78
79 if __name__ == "__main__":
80         ImuBrickv2()