imu_node.py 5.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162
  1. #!/usr/bin/env python
  2. import smbus
  3. import rospy
  4. import string
  5. import math
  6. import time
  7. import sys
  8. from tf.transformations import quaternion_from_euler
  9. from dynamic_reconfigure.server import Server
  10. from rasp_imu_hat_6dof.cfg import imuConfig
  11. from sensor_msgs.msg import Imu
  12. degrees2rad = math.pi/180.0
  13. imu_yaw_calibration = 0.0
  14. # Callback for dynamic reconfigure requests
  15. def reconfig_callback(config, level):
  16. global imu_yaw_calibration
  17. imu_yaw_calibration = config['yaw_calibration']
  18. rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
  19. return config
  20. rospy.init_node("imu_node")
  21. #Get DIY param
  22. topic_name = rospy.get_param('~pub_topic', 'imu')
  23. link_name = rospy.get_param('~link_name', 'base_imu_link')
  24. pub_hz = rospy.get_param('~pub_hz', '10')
  25. pub = rospy.Publisher(topic_name, Imu, queue_size=1)
  26. srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback
  27. imuMsg = Imu()
  28. # Orientation covariance estimation:
  29. # Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
  30. # Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
  31. # static roll/pitch error of 0.8%, owing to gravity orientation sensing
  32. # error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
  33. # so set all covariances the same.
  34. imuMsg.orientation_covariance = [
  35. 0.0025 , 0 , 0,
  36. 0, 0.0025, 0,
  37. 0, 0, 0.0025
  38. ]
  39. # Angular velocity covariance estimation:
  40. # Observed gyro noise: 4 counts => 0.28 degrees/sec
  41. # nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
  42. # Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
  43. imuMsg.angular_velocity_covariance = [
  44. 0.02, 0 , 0,
  45. 0 , 0.02, 0,
  46. 0 , 0 , 0.02
  47. ]
  48. # linear acceleration covariance estimation:
  49. # observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
  50. # nonliniarity spec: 0.5% of full scale => 0.2m/s^2
  51. # Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
  52. imuMsg.linear_acceleration_covariance = [
  53. 0.04 , 0 , 0,
  54. 0 , 0.04, 0,
  55. 0 , 0 , 0.04
  56. ]
  57. seq=0
  58. accel_factor = 9.806/256.0 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
  59. class MyIMU(object):
  60. def __init__(self, addr):
  61. self.addr = addr
  62. self.i2c = smbus.SMBus(1)
  63. def get_YPRAG(self):
  64. try:
  65. roll_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3d, 2)
  66. pitch_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3e, 2)
  67. yaw_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3f, 2)
  68. ax_tmp = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
  69. ay_tmp = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
  70. az_tmp = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
  71. gx_tmp = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
  72. gy_tmp = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)
  73. gz_tmp = self.i2c.read_i2c_block_data(self.addr, 0x39, 2)
  74. except IOError:
  75. rospy.logerr("Read IMU YPRAG date error !")
  76. else:
  77. self.raw_roll = float(((roll_tmp[1]<<8) |roll_tmp[0])/32768.0*180.0)
  78. self.raw_pitch = float(((pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
  79. self.raw_yaw = (yaw_tmp[1] << 8 | yaw_tmp[0])/32768.0*180
  80. self.raw_ax = float(((ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
  81. self.raw_ay = float(((ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
  82. self.raw_az = float(((az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
  83. self.raw_gx = float(((gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
  84. self.raw_gy = float(((gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
  85. self.raw_gz = float(((gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
  86. def get_quatern(self):
  87. try:
  88. q0 = self.i2c.read_i2c_block_data(self.addr, 0x51, 2)
  89. q1 = self.i2c.read_i2c_block_data(self.addr, 0x52, 2)
  90. q2 = self.i2c.read_i2c_block_data(self.addr, 0x53, 2)
  91. q3 = self.i2c.read_i2c_block_data(self.addr, 0x54, 2)
  92. except IOError:
  93. rospy.logerr("Read IMU quaternion date error !")
  94. else:
  95. self.raw_q0 = float(((q0[1]<<8)|q0[0])/32768.0)
  96. self.raw_q1 = float(((q1[1]<<8)|q1[0])/32768.0)
  97. self.raw_q2 = float(((q2[1]<<8)|q2[0])/32768.0)
  98. self.raw_q3 = float(((q3[1]<<8)|q3[0])/32768.0)
  99. myIMU = MyIMU(0x50)
  100. rate = rospy.Rate(pub_hz)
  101. while not rospy.is_shutdown():
  102. myIMU.get_YPRAG()
  103. yaw_deg = float(myIMU.raw_yaw)
  104. yaw_deg = yaw_deg + imu_yaw_calibration
  105. if yaw_deg >= 180.0:
  106. yaw_deg -= 360.0
  107. #rospy.loginfo("yaw_deg: %f", yaw_deg)
  108. yaw = yaw_deg*degrees2rad
  109. #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
  110. pitch = float(myIMU.raw_pitch)*degrees2rad
  111. roll = float(myIMU.raw_roll)*degrees2rad
  112. #rospy.loginfo("yaw:%f pitch:%f rool:%f", yaw, pitch, roll)
  113. # Publish message
  114. imuMsg.linear_acceleration.x = float(myIMU.raw_ax)*accel_factor
  115. imuMsg.linear_acceleration.y = float(myIMU.raw_ay)*accel_factor
  116. imuMsg.linear_acceleration.z = float(myIMU.raw_az)*accel_factor
  117. imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
  118. imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
  119. imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
  120. #myIMU.get_quatern()
  121. q = quaternion_from_euler(roll, pitch, yaw)
  122. imuMsg.orientation.x = q[0]
  123. imuMsg.orientation.y = q[1]
  124. imuMsg.orientation.z = q[2]
  125. imuMsg.orientation.w = q[3]
  126. imuMsg.header.stamp= rospy.Time.now()
  127. imuMsg.header.frame_id = link_name
  128. imuMsg.header.seq = seq
  129. pub.publish(imuMsg)
  130. seq = seq + 1
  131. rate.sleep()