imu_node.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (c) 2012, Tang Tiong Yew
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions are met:
8 #
9 # * Redistributions of source code must retain the above copyright
10 # notice, this list of conditions and the following disclaimer.
11 # * Redistributions in binary form must reproduce the above copyright
12 # notice, this list of conditions and the following disclaimer in the
13 # documentation and/or other materials provided with the distribution.
14 # * Neither the name of the Willow Garage, Inc. nor the names of its
15 # contributors may be used to endorse or promote products derived from
16 # this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 # POSSIBILITY OF SUCH DAMAGE.
29 
30 import rospy
31 import serial
32 import string
33 import math
34 import sys
35 
36 #from time import time
37 from sensor_msgs.msg import Imu
38 from tf.transformations import quaternion_from_euler
39 from dynamic_reconfigure.server import Server
40 from razor_imu_9dof.cfg import imuConfig
41 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
42 
43 degrees2rad = math.pi/180.0
44 imu_yaw_calibration = 0.0
45 
46 # Callback for dynamic reconfigure requests
47 def reconfig_callback(config, level):
48  global imu_yaw_calibration
49  rospy.loginfo("""Reconfigure request for yaw_calibration: %d""" %(config['yaw_calibration']))
50  #if imu_yaw_calibration != config('yaw_calibration'):
51  imu_yaw_calibration = config['yaw_calibration']
52  rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
53  return config
54 
55 rospy.init_node("razor_node")
56 
57 imuMsg = Imu()
58 
59 # Orientation covariance estimation:
60 # Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
61 # Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
62 # Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
63 # cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
64 # i.e. variance in yaw: 0.0025
65 # Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
66 # static roll/pitch error of 0.8%, owing to gravity orientation sensing
67 # error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
68 # so set all covariances the same.
69 imuMsg.orientation_covariance = [
70 0.0025 , 0 , 0,
71 0, 0.0025, 0,
72 0, 0, 0.0025
73 ]
74 
75 # Angular velocity covariance estimation:
76 # Observed gyro noise: 4 counts => 0.28 degrees/sec
77 # nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
78 # Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
79 imuMsg.angular_velocity_covariance = [
80 0.02, 0 , 0,
81 0 , 0.02, 0,
82 0 , 0 , 0.02
83 ]
84 
85 # linear acceleration covariance estimation:
86 # observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
87 # nonliniarity spec: 0.5% of full scale => 0.2m/s^2
88 # Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
89 imuMsg.linear_acceleration_covariance = [
90 0.04 , 0 , 0,
91 0 , 0.04, 0,
92 0 , 0 , 0.04
93 ]
94 
95 # read basic information
96 port = rospy.get_param('~port', '/dev/ttyUSB0')
97 topic = rospy.get_param('~topic', 'imu')
98 frame_id = rospy.get_param('~frame_id', 'base_imu_link')
99 
100 # read calibration parameters
101 
102 # accelerometer
103 accel_x_min = rospy.get_param('~accel_x_min', -250.0)
104 accel_x_max = rospy.get_param('~accel_x_max', 250.0)
105 accel_y_min = rospy.get_param('~accel_y_min', -250.0)
106 accel_y_max = rospy.get_param('~accel_y_max', 250.0)
107 accel_z_min = rospy.get_param('~accel_z_min', -250.0)
108 accel_z_max = rospy.get_param('~accel_z_max', 250.0)
109 
110 # magnetometer
111 magn_x_min = rospy.get_param('~magn_x_min', -600.0)
112 magn_x_max = rospy.get_param('~magn_x_max', 600.0)
113 magn_y_min = rospy.get_param('~magn_y_min', -600.0)
114 magn_y_max = rospy.get_param('~magn_y_max', 600.0)
115 magn_z_min = rospy.get_param('~magn_z_min', -600.0)
116 magn_z_max = rospy.get_param('~magn_z_max', 600.0)
117 calibration_magn_use_extended = rospy.get_param('~calibration_magn_use_extended', False)
118 magn_ellipsoid_center = rospy.get_param('~magn_ellipsoid_center', [0, 0, 0])
119 magn_ellipsoid_transform = rospy.get_param('~magn_ellipsoid_transform', [[0, 0, 0], [0, 0, 0], [0, 0, 0]])
120 imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0)
121 
122 # gyroscope
123 gyro_average_offset_x = rospy.get_param('~gyro_average_offset_x', 0.0)
124 gyro_average_offset_y = rospy.get_param('~gyro_average_offset_y', 0.0)
125 gyro_average_offset_z = rospy.get_param('~gyro_average_offset_z', 0.0)
126 
127 #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)
128 #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)
129 #rospy.loginfo("%s %s %s", str(calibration_magn_use_extended), str(magn_ellipsoid_center), str(magn_ellipsoid_transform[0][0]))
130 #rospy.loginfo("%f %f %f", gyro_average_offset_x, gyro_average_offset_y, gyro_average_offset_z)
131 
132 pub = rospy.Publisher(topic, Imu, queue_size=1)
133 srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback
134 diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
135 diag_pub_time = rospy.get_time();
136 
137 # Check your COM port and baud rate
138 rospy.loginfo("Opening %s...", port)
139 try:
140  ser = serial.Serial(port=port, baudrate=57600, timeout=1)
141  #ser = serial.Serial(port=port, baudrate=57600, timeout=1, rtscts=True, dsrdtr=True) # For compatibility with some virtual serial ports (e.g. created by socat) in Python 2.7
142 except serial.serialutil.SerialException:
143  rospy.logerr("IMU not found at port "+port + ". Did you specify the correct port in the launch file?")
144  #exit
145  sys.exit(2)
146 
147 roll=0
148 pitch=0
149 yaw=0
150 seq=0
151 accel_factor = 9.806 / 256.0 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
152 rospy.loginfo("Giving the razor IMU board 5 seconds to boot...")
153 rospy.sleep(5) # Sleep for 5 seconds to wait for the board to boot
154 
155 ### configure board ###
156 #stop datastream
157 ser.write(('#o0').encode("utf-8"))
158 
159 #discard old input
160 #automatic flush - NOT WORKING
161 #ser.flushInput() #discard old input, still in invalid format
162 #flush manually, as above command is not working
163 discard = ser.readlines()
164 
165 #set output mode
166 ser.write(('#ox').encode("utf-8")) # To start display angle and sensor reading in text
167 
168 rospy.loginfo("Writing calibration values to razor IMU board...")
169 #set calibration values
170 ser.write(('#caxm' + str(accel_x_min)).encode("utf-8"))
171 ser.write(('#caxM' + str(accel_x_max)).encode("utf-8"))
172 ser.write(('#caym' + str(accel_y_min)).encode("utf-8"))
173 ser.write(('#cayM' + str(accel_y_max)).encode("utf-8"))
174 ser.write(('#cazm' + str(accel_z_min)).encode("utf-8"))
175 ser.write(('#cazM' + str(accel_z_max)).encode("utf-8"))
176 
177 if (not calibration_magn_use_extended):
178  ser.write(('#cmxm' + str(magn_x_min)).encode("utf-8"))
179  ser.write(('#cmxM' + str(magn_x_max)).encode("utf-8"))
180  ser.write(('#cmym' + str(magn_y_min)).encode("utf-8"))
181  ser.write(('#cmyM' + str(magn_y_max)).encode("utf-8"))
182  ser.write(('#cmzm' + str(magn_z_min)).encode("utf-8"))
183  ser.write(('#cmzM' + str(magn_z_max)).encode("utf-8"))
184 else:
185  ser.write(('#ccx' + str(magn_ellipsoid_center[0])).encode("utf-8"))
186  ser.write(('#ccy' + str(magn_ellipsoid_center[1])).encode("utf-8"))
187  ser.write(('#ccz' + str(magn_ellipsoid_center[2])).encode("utf-8"))
188  ser.write(('#ctxX' + str(magn_ellipsoid_transform[0][0])).encode("utf-8"))
189  ser.write(('#ctxY' + str(magn_ellipsoid_transform[0][1])).encode("utf-8"))
190  ser.write(('#ctxZ' + str(magn_ellipsoid_transform[0][2])).encode("utf-8"))
191  ser.write(('#ctyX' + str(magn_ellipsoid_transform[1][0])).encode("utf-8"))
192  ser.write(('#ctyY' + str(magn_ellipsoid_transform[1][1])).encode("utf-8"))
193  ser.write(('#ctyZ' + str(magn_ellipsoid_transform[1][2])).encode("utf-8"))
194  ser.write(('#ctzX' + str(magn_ellipsoid_transform[2][0])).encode("utf-8"))
195  ser.write(('#ctzY' + str(magn_ellipsoid_transform[2][1])).encode("utf-8"))
196  ser.write(('#ctzZ' + str(magn_ellipsoid_transform[2][2])).encode("utf-8"))
197 
198 ser.write(('#cgx' + str(gyro_average_offset_x)).encode("utf-8"))
199 ser.write(('#cgy' + str(gyro_average_offset_y)).encode("utf-8"))
200 ser.write(('#cgz' + str(gyro_average_offset_z)).encode("utf-8"))
201 
202 #print calibration values for verification by user
203 ser.flushInput()
204 ser.write(('#p').encode("utf-8"))
205 calib_data = ser.readlines()
206 calib_data_print = "Printing set calibration values:\r\n"
207 for row in calib_data:
208  line = bytearray(row).decode("utf-8")
209  calib_data_print += line
210 rospy.loginfo(calib_data_print)
211 
212 #start datastream
213 ser.write(('#o1').encode("utf-8"))
214 
215 #automatic flush - NOT WORKING
216 #ser.flushInput() #discard old input, still in invalid format
217 #flush manually, as above command is not working - it breaks the serial connection
218 rospy.loginfo("Flushing first 200 IMU entries...")
219 for x in range(0, 200):
220  line = bytearray(ser.readline()).decode("utf-8")
221 rospy.loginfo("Publishing IMU data...")
222 #f = open("raw_imu_data.log", 'w')
223 
224 errcount = 0
225 while not rospy.is_shutdown():
226  if (errcount > 10):
227  break
228  line = bytearray(ser.readline()).decode("utf-8")
229  if ((line.find("#YPRAG=") == -1) or (line.find("\r\n") == -1)):
230  rospy.logwarn("Bad IMU data or bad sync")
231  errcount = errcount+1
232  continue
233  line = line.replace("#YPRAG=","") # Delete "#YPRAG="
234  #f.write(line) # Write to the output log file
235  line = line.replace("\r\n","") # Delete "\r\n"
236  words = line.split(",") # Fields split
237  if len(words) != 9:
238  rospy.logwarn("Bad IMU data or bad sync")
239  errcount = errcount+1
240  continue
241  else:
242  errcount = 0
243  #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103)
244  yaw_deg = -float(words[0])
245  yaw_deg = yaw_deg + imu_yaw_calibration
246  if yaw_deg > 180.0:
247  yaw_deg = yaw_deg - 360.0
248  if yaw_deg < -180.0:
249  yaw_deg = yaw_deg + 360.0
250  yaw = yaw_deg*degrees2rad
251  #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
252  pitch = -float(words[1])*degrees2rad
253  roll = float(words[2])*degrees2rad
254 
255  # Publish message
256  # AHRS firmware accelerations are negated
257  # This means y and z are correct for ROS, but x needs reversing
258  imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor
259  imuMsg.linear_acceleration.y = float(words[4]) * accel_factor
260  imuMsg.linear_acceleration.z = float(words[5]) * accel_factor
261 
262  imuMsg.angular_velocity.x = float(words[6])
263  #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
264  imuMsg.angular_velocity.y = -float(words[7])
265  #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103)
266  imuMsg.angular_velocity.z = -float(words[8])
267 
268  q = quaternion_from_euler(roll,pitch,yaw)
269  imuMsg.orientation.x = q[0]
270  imuMsg.orientation.y = q[1]
271  imuMsg.orientation.z = q[2]
272  imuMsg.orientation.w = q[3]
273  imuMsg.header.stamp= rospy.Time.now()
274  imuMsg.header.frame_id = frame_id
275  imuMsg.header.seq = seq
276  seq = seq + 1
277  pub.publish(imuMsg)
278 
279  if (diag_pub_time < rospy.get_time()) :
280  diag_pub_time += 1
281  diag_arr = DiagnosticArray()
282  diag_arr.header.stamp = rospy.get_rostime()
283  diag_arr.header.frame_id = '1'
284  diag_msg = DiagnosticStatus()
285  diag_msg.name = 'Razor_Imu'
286  diag_msg.level = DiagnosticStatus.OK
287  diag_msg.message = 'Received AHRS measurement'
288  diag_msg.values.append(KeyValue('roll (deg)',
289  str(roll*(180.0/math.pi))))
290  diag_msg.values.append(KeyValue('pitch (deg)',
291  str(pitch*(180.0/math.pi))))
292  diag_msg.values.append(KeyValue('yaw (deg)',
293  str(yaw*(180.0/math.pi))))
294  diag_msg.values.append(KeyValue('sequence number', str(seq)))
295  diag_arr.status.append(diag_msg)
296  diag_pub.publish(diag_arr)
297 
298 ser.close
299 
300 #f.close
301 
302 if (errcount > 10):
303  sys.exit(10)
display_3D_visualization.range
range
Definition: display_3D_visualization.py:68
imu_node.reconfig_callback
def reconfig_callback(config, level)
Definition: imu_node.py:47

razor_imu_9dof
Author(s): Tang Tiong Yew, Kristof Robot, Paul Bouchier, Peter Bartz
autogenerated on Sat May 1 2021 02:09:10