1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35 import rospy
36 import math
37 import copy
38 import sensor_msgs.msg
39 import PyKDL
40
43 self.cal_offset = 0.0
44 self.orientation = 0.0
45 self.cal_buffer =[]
46 self.cal_buffer_length = 1000
47 self.imu_data = sensor_msgs.msg.Imu(header=rospy.Header(frame_id="gyro_link"))
48 self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
49 self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
50 self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0]
51 self.gyro_measurement_range = rospy.get_param('~gyro_measurement_range', 150.0)
52 self.gyro_scale_correction = rospy.get_param('~gyro_scale_correction', 1.35)
53 self.imu_pub = rospy.Publisher('imu/data', sensor_msgs.msg.Imu)
54 self.imu_pub_raw = rospy.Publisher('imu/raw', sensor_msgs.msg.Imu)
55
57 self.gyro_measurement_range = rospy.get_param('~gyro_measurement_range', 150.0)
58 self.gyro_scale_correction = rospy.get_param('~gyro_scale_correction', 1.35)
59 rospy.loginfo('self.gyro_measurement_range %f'%self.gyro_measurement_range)
60 rospy.loginfo('self.gyro_scale_correction %f'%self.gyro_scale_correction)
61
63
64
65 if sensor_state.requested_right_velocity == 0 and \
66 sensor_state.requested_left_velocity == 0 and \
67 sensor_state.distance == 0:
68
69 self.cal_buffer.append(sensor_state.user_analog_input)
70 if len(self.cal_buffer) > self.cal_buffer_length:
71 del self.cal_buffer[:-self.cal_buffer_length]
72 self.cal_offset = sum(self.cal_buffer)/len(self.cal_buffer)
73
74 - def publish(self, sensor_state, last_time):
75 if self.cal_offset == 0:
76 return
77
78 current_time = sensor_state.header.stamp
79 dt = (current_time - last_time).to_sec()
80 past_orientation = self.orientation
81 self.imu_data.header.stamp = sensor_state.header.stamp
82 self.imu_data.angular_velocity.z = (float(sensor_state.user_analog_input)-self.cal_offset)/self.cal_offset*self.gyro_measurement_range*(math.pi/180.0)*self.gyro_scale_correction
83
84 self.imu_data.angular_velocity.z = -1.0*self.imu_data.angular_velocity.z
85 self.orientation += self.imu_data.angular_velocity.z * dt
86
87 (self.imu_data.orientation.x, self.imu_data.orientation.y, self.imu_data.orientation.z, self.imu_data.orientation.w) = PyKDL.Rotation.RotZ(self.orientation).GetQuaternion()
88 self.imu_pub.publish(self.imu_data)
89
90 self.imu_data.header.stamp = sensor_state.header.stamp
91 self.imu_data.angular_velocity.z = (float(sensor_state.user_analog_input)/self.gyro_measurement_range*(math.pi/180.0)*self.gyro_scale_correction)
92
93 self.imu_data.angular_velocity.z = -1.0*self.imu_data.angular_velocity.z
94 raw_orientation = past_orientation + self.imu_data.angular_velocity.z * dt
95
96 (self.imu_data.orientation.x, self.imu_data.orientation.y, self.imu_data.orientation.z, self.imu_data.orientation.w) = PyKDL.Rotation.RotZ(raw_orientation).GetQuaternion()
97 self.imu_pub_raw.publish(self.imu_data)
98