scripts/tinkerforge_imu2.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 HOST = "localhost"
   5 PORT = 4223
   6 UID = "6DdNSn"
   7
   8 import rospy
   9 import prctl
  10 from sensor_msgs.msg import Imu
  11 from tinkerforge.ip_connection import IPConnection
  12 from tinkerforge.brick_imu_v2 import BrickIMUV2
  13
  14 class ImuBrickv2:
  15         def __init__(self):
  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)
  19
  20                 ipcon = IPConnection() # Create IP connection
  21                 imu = BrickIMUV2(UID, ipcon) # Create device object
  22                 ipcon.connect(HOST, PORT) # Connect to brickd
  23
  24                 imu.leds_off()
  25                 #imu.disable_status_led()
  26
  27                 imu.register_callback(imu.CALLBACK_ALL_DATA, self.cb_all_data)
  28                 imu.set_all_data_period(20) # ms
  29
  30                 rospy.spin()
  31                 ipcon.disconnect()
  32
  33
  34         def cb_all_data(self, acceleration, magnetic_field, angular_velocity, euler_angle, quaternion, linear_acceleration, gravity_vector, temperature, calibration_status):
  35                 msg = Imu()
  36                 msg.header.stamp = rospy.Time.now()
  37                 msg.header.frame_id = "base_imu_link"
  38
  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 = [
  47                         0.008, 0     , 0,
  48                         0    , 0.008, 0,
  49                         0    , 0    , 0.008
  50                 ]
  51
  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 = [
  57                         0.02, 0   , 0,
  58                         0   , 0.02, 0,
  59                         0   , 0   , 0.02
  60                 ]
  61
  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 = [
  70                         0.04, 0    , 0,
  71                         0    , 0.04, 0,
  72                         0    , 0    , 0.04
  73                 ]
  74
  75                 self.pub_imu.publish(msg)
  76
  77
  78 if __name__ == "__main__":
  79         ImuBrickv2()