2 # -*- coding: iso-8859-15 -*-
10 from sensor_msgs.msg import Imu
11 from tinkerforge.ip_connection import IPConnection
12 from tinkerforge.brick_imu_v2 import BrickIMUV2
16 rospy.init_node('imu_brick_v2')
17 prctl.set_name("imu_brick_v2")
18 self.pub_imu = rospy.Publisher("imu", Imu, queue_size=16)
20 ipcon = IPConnection() # Create IP connection
21 imu = BrickIMUV2(UID, ipcon) # Create device object
22 ipcon.connect(HOST, PORT) # Connect to brickd
25 #imu.disable_status_led()
27 imu.register_callback(imu.CALLBACK_ALL_DATA, self.cb_all_data)
28 imu.set_all_data_period(20) # ms
34 def cb_all_data(self, acceleration, magnetic_field, angular_velocity, euler_angle, quaternion, linear_acceleration, gravity_vector, temperature, calibration_status):
36 msg.header.stamp = rospy.Time.now()
37 msg.header.frame_id = "base_imu_link"
39 msg.orientation.x = quaternion[1]/16383.0
40 msg.orientation.y = quaternion[2]/16383.0
41 msg.orientation.z = quaternion[3]/16383.0
42 msg.orientation.w = quaternion[0]/16383.0
43 # Observed orientation variance: 0.0 (10k samples)
44 # Magnometer heading accuracy is +-2.5 deg => 0.088 rad
45 # With heading accuracy as std dev, variance = 0.088^2 = 0.008
46 msg.orientation_covariance = [
52 msg.angular_velocity.x = angular_velocity[0]/16.0
53 msg.angular_velocity.y = angular_velocity[1]/16.0
54 msg.angular_velocity.z = angular_velocity[2]/16.0
55 # Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
56 msg.angular_velocity_covariance = [
62 msg.linear_acceleration.x = linear_acceleration[0]/100.0
63 msg.linear_acceleration.y = linear_acceleration[1]/100.0
64 msg.linear_acceleration.z = linear_acceleration[2]/100.0
65 # Observed linear acceleration variance: 0.001532 (10k samples)
66 # Calculation for variance taken from razor imu:
67 # nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2
68 # Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
69 msg.linear_acceleration_covariance = [
75 self.pub_imu.publish(msg)
78 if __name__ == "__main__":