razor_imu_9dof: imu_node.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2012, Tang Tiong Yew
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #    * Redistributions of source code must retain the above copyright
00010 #      notice, this list of conditions and the following disclaimer.
00011 #    * Redistributions in binary form must reproduce the above copyright
00012 #      notice, this list of conditions and the following disclaimer in the
00013 #      documentation and/or other materials provided with the distribution.
00014 #    * Neither the name of the Willow Garage, Inc. nor the names of its
00015 #      contributors may be used to endorse or promote products derived from
00016 #       this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 import rospy
00031 import serial
00032 import string
00033 import math
00034 import sys
00035 
00036 #from time import time
00037 from sensor_msgs.msg import Imu
00038 from tf.transformations import quaternion_from_euler
00039 from dynamic_reconfigure.server import Server
00040 from razor_imu_9dof.cfg import imuConfig
00041 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00042 
00043 degrees2rad = math.pi/180.0
00044 imu_yaw_calibration = 0.0
00045 
00046 # Callback for dynamic reconfigure requests
00047 def reconfig_callback(config, level):
00048     global imu_yaw_calibration
00049     rospy.loginfo("""Reconfigure request for yaw_calibration: %d""" %(config['yaw_calibration']))
00050     #if imu_yaw_calibration != config('yaw_calibration'):
00051     imu_yaw_calibration = config['yaw_calibration']
00052     rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
00053     return config
00054 
00055 rospy.init_node("razor_node")
00056 #We only care about the most recent measurement, i.e. queue_size=1
00057 pub = rospy.Publisher('imu', Imu, queue_size=1)
00058 srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
00059 diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
00060 diag_pub_time = rospy.get_time();
00061 
00062 imuMsg = Imu()
00063 
00064 # Orientation covariance estimation:
00065 # Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
00066 # Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
00067 # Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
00068 # cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
00069 # i.e. variance in yaw: 0.0025
00070 # Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
00071 # static roll/pitch error of 0.8%, owing to gravity orientation sensing
00072 # error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
00073 # so set all covariances the same.
00074 imuMsg.orientation_covariance = [
00075 0.0025 , 0 , 0,
00076 0, 0.0025, 0,
00077 0, 0, 0.0025
00078 ]
00079 
00080 # Angular velocity covariance estimation:
00081 # Observed gyro noise: 4 counts => 0.28 degrees/sec
00082 # nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
00083 # Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
00084 imuMsg.angular_velocity_covariance = [
00085 0.02, 0 , 0,
00086 0 , 0.02, 0,
00087 0 , 0 , 0.02
00088 ]
00089 
00090 # linear acceleration covariance estimation:
00091 # observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
00092 # nonliniarity spec: 0.5% of full scale => 0.2m/s^2
00093 # Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
00094 imuMsg.linear_acceleration_covariance = [
00095 0.04 , 0 , 0,
00096 0 , 0.04, 0,
00097 0 , 0 , 0.04
00098 ]
00099 
00100 default_port='/dev/ttyUSB0'
00101 port = rospy.get_param('~port', default_port)
00102 
00103 #read calibration parameters
00104 port = rospy.get_param('~port', default_port)
00105 
00106 #accelerometer
00107 accel_x_min = rospy.get_param('~accel_x_min', -250.0)
00108 accel_x_max = rospy.get_param('~accel_x_max', 250.0)
00109 accel_y_min = rospy.get_param('~accel_y_min', -250.0)
00110 accel_y_max = rospy.get_param('~accel_y_max', 250.0)
00111 accel_z_min = rospy.get_param('~accel_z_min', -250.0)
00112 accel_z_max = rospy.get_param('~accel_z_max', 250.0)
00113 
00114 # magnetometer
00115 magn_x_min = rospy.get_param('~magn_x_min', -600.0)
00116 magn_x_max = rospy.get_param('~magn_x_max', 600.0)
00117 magn_y_min = rospy.get_param('~magn_y_min', -600.0)
00118 magn_y_max = rospy.get_param('~magn_y_max', 600.0)
00119 magn_z_min = rospy.get_param('~magn_z_min', -600.0)
00120 magn_z_max = rospy.get_param('~magn_z_max', 600.0)
00121 calibration_magn_use_extended = rospy.get_param('~calibration_magn_use_extended', False)
00122 magn_ellipsoid_center = rospy.get_param('~magn_ellipsoid_center', [0, 0, 0])
00123 magn_ellipsoid_transform = rospy.get_param('~magn_ellipsoid_transform', [[0, 0, 0], [0, 0, 0], [0, 0, 0]])
00124 imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0)
00125 
00126 # gyroscope
00127 gyro_average_offset_x = rospy.get_param('~gyro_average_offset_x', 0.0)
00128 gyro_average_offset_y = rospy.get_param('~gyro_average_offset_y', 0.0)
00129 gyro_average_offset_z = rospy.get_param('~gyro_average_offset_z', 0.0)
00130 
00131 #rospy.loginfo("%f %f %f %f %f %f", accel_x_min, accel_x_max, accel_y_min, accel_y_max, accel_z_min, accel_z_max)
00132 #rospy.loginfo("%f %f %f %f %f %f", magn_x_min, magn_x_max, magn_y_min, magn_y_max, magn_z_min, magn_z_max)
00133 #rospy.loginfo("%s %s %s", str(calibration_magn_use_extended), str(magn_ellipsoid_center), str(magn_ellipsoid_transform[0][0]))
00134 #rospy.loginfo("%f %f %f", gyro_average_offset_x, gyro_average_offset_y, gyro_average_offset_z)
00135 
00136 # Check your COM port and baud rate
00137 rospy.loginfo("Opening %s...", port)
00138 try:
00139     ser = serial.Serial(port=port, baudrate=57600, timeout=1)
00140 except serial.serialutil.SerialException:
00141     rospy.logerr("IMU not found at port "+port + ". Did you specify the correct port in the launch file?")
00142     #exit
00143     sys.exit(0)
00144 
00145 roll=0
00146 pitch=0
00147 yaw=0
00148 seq=0
00149 accel_factor = 9.806 / 256.0    # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
00150 rospy.loginfo("Giving the razor IMU board 5 seconds to boot...")
00151 rospy.sleep(5) # Sleep for 5 seconds to wait for the board to boot
00152 
00153 ### configure board ###
00154 #stop datastream
00155 ser.write('#o0' + chr(13))
00156 
00157 #discard old input
00158 #automatic flush - NOT WORKING
00159 #ser.flushInput()  #discard old input, still in invalid format
00160 #flush manually, as above command is not working
00161 discard = ser.readlines() 
00162 
00163 #set output mode
00164 ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text
00165 
00166 rospy.loginfo("Writing calibration values to razor IMU board...")
00167 #set calibration values
00168 ser.write('#caxm' + str(accel_x_min) + chr(13))
00169 ser.write('#caxM' + str(accel_x_max) + chr(13))
00170 ser.write('#caym' + str(accel_y_min) + chr(13))
00171 ser.write('#cayM' + str(accel_y_max) + chr(13))
00172 ser.write('#cazm' + str(accel_z_min) + chr(13))
00173 ser.write('#cazM' + str(accel_z_max) + chr(13))
00174 
00175 if (not calibration_magn_use_extended):
00176     ser.write('#cmxm' + str(magn_x_min) + chr(13))
00177     ser.write('#cmxM' + str(magn_x_max) + chr(13))
00178     ser.write('#cmym' + str(magn_y_min) + chr(13))
00179     ser.write('#cmyM' + str(magn_y_max) + chr(13))
00180     ser.write('#cmzm' + str(magn_z_min) + chr(13))
00181     ser.write('#cmzM' + str(magn_z_max) + chr(13))
00182 else:
00183     ser.write('#ccx' + str(magn_ellipsoid_center[0]) + chr(13))
00184     ser.write('#ccy' + str(magn_ellipsoid_center[1]) + chr(13))
00185     ser.write('#ccz' + str(magn_ellipsoid_center[2]) + chr(13))
00186     ser.write('#ctxX' + str(magn_ellipsoid_transform[0][0]) + chr(13))
00187     ser.write('#ctxY' + str(magn_ellipsoid_transform[0][1]) + chr(13))
00188     ser.write('#ctxZ' + str(magn_ellipsoid_transform[0][2]) + chr(13))
00189     ser.write('#ctyX' + str(magn_ellipsoid_transform[1][0]) + chr(13))
00190     ser.write('#ctyY' + str(magn_ellipsoid_transform[1][1]) + chr(13))
00191     ser.write('#ctyZ' + str(magn_ellipsoid_transform[1][2]) + chr(13))
00192     ser.write('#ctzX' + str(magn_ellipsoid_transform[2][0]) + chr(13))
00193     ser.write('#ctzY' + str(magn_ellipsoid_transform[2][1]) + chr(13))
00194     ser.write('#ctzZ' + str(magn_ellipsoid_transform[2][2]) + chr(13))
00195 
00196 ser.write('#cgx' + str(gyro_average_offset_x) + chr(13))
00197 ser.write('#cgy' + str(gyro_average_offset_y) + chr(13))
00198 ser.write('#cgz' + str(gyro_average_offset_z) + chr(13))
00199 
00200 #print calibration values for verification by user
00201 ser.flushInput()
00202 ser.write('#p' + chr(13))
00203 calib_data = ser.readlines()
00204 calib_data_print = "Printing set calibration values:\r\n"
00205 for line in calib_data:
00206     calib_data_print += line
00207 rospy.loginfo(calib_data_print)
00208 
00209 #start datastream
00210 ser.write('#o1' + chr(13))
00211 
00212 #automatic flush - NOT WORKING
00213 #ser.flushInput()  #discard old input, still in invalid format
00214 #flush manually, as above command is not working - it breaks the serial connection
00215 rospy.loginfo("Flushing first 200 IMU entries...")
00216 for x in range(0, 200):
00217     line = ser.readline()
00218 rospy.loginfo("Publishing IMU data...")
00219 #f = open("raw_imu_data.log", 'w')
00220 
00221 while not rospy.is_shutdown():
00222     line = ser.readline()
00223     line = line.replace("#YPRAG=","")   # Delete "#YPRAG="
00224     #f.write(line)                     # Write to the output log file
00225     words = string.split(line,",")    # Fields split
00226     if len(words) > 2:
00227         #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103)
00228         yaw_deg = -float(words[0])
00229         yaw_deg = yaw_deg + imu_yaw_calibration
00230         if yaw_deg > 180.0:
00231             yaw_deg = yaw_deg - 360.0
00232         if yaw_deg < -180.0:
00233             yaw_deg = yaw_deg + 360.0
00234         yaw = yaw_deg*degrees2rad
00235         #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
00236         pitch = -float(words[1])*degrees2rad
00237         roll = float(words[2])*degrees2rad
00238 
00239         # Publish message
00240         # AHRS firmware accelerations are negated
00241         # This means y and z are correct for ROS, but x needs reversing
00242         imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor
00243         imuMsg.linear_acceleration.y = float(words[4]) * accel_factor
00244         imuMsg.linear_acceleration.z = float(words[5]) * accel_factor
00245 
00246         imuMsg.angular_velocity.x = float(words[6])
00247         #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
00248         imuMsg.angular_velocity.y = -float(words[7])
00249         #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) 
00250         imuMsg.angular_velocity.z = -float(words[8])
00251 
00252     q = quaternion_from_euler(roll,pitch,yaw)
00253     imuMsg.orientation.x = q[0]
00254     imuMsg.orientation.y = q[1]
00255     imuMsg.orientation.z = q[2]
00256     imuMsg.orientation.w = q[3]
00257     imuMsg.header.stamp= rospy.Time.now()
00258     imuMsg.header.frame_id = 'base_imu_link'
00259     imuMsg.header.seq = seq
00260     seq = seq + 1
00261     pub.publish(imuMsg)
00262 
00263     if (diag_pub_time < rospy.get_time()) :
00264         diag_pub_time += 1
00265         diag_arr = DiagnosticArray()
00266         diag_arr.header.stamp = rospy.get_rostime()
00267         diag_arr.header.frame_id = '1'
00268         diag_msg = DiagnosticStatus()
00269         diag_msg.name = 'Razor_Imu'
00270         diag_msg.level = DiagnosticStatus.OK
00271         diag_msg.message = 'Received AHRS measurement'
00272         diag_msg.values.append(KeyValue('roll (deg)',
00273                                 str(roll*(180.0/math.pi))))
00274         diag_msg.values.append(KeyValue('pitch (deg)',
00275                                 str(pitch*(180.0/math.pi))))
00276         diag_msg.values.append(KeyValue('yaw (deg)',
00277                                 str(yaw*(180.0/math.pi))))
00278         diag_msg.values.append(KeyValue('sequence number', str(seq)))
00279         diag_arr.status.append(diag_msg)
00280         diag_pub.publish(diag_arr)
00281         
00282 ser.close
00283 #f.close