Source Code for Module eddiebot_node.gyro

 1  # Software License Agreement (BSD License) 
 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 
 8  # are met: 
 9  # 
10  #  * Redistributions of source code must retain the above copyright 
11  #    notice, this list of conditions and the following disclaimer. 
12  #  * Redistributions in binary form must reproduce the above 
13  #    copyright notice, this list of conditions and the following 
14  #    disclaimer in the documentation and/or other materials provided 
15  #    with the distribution. 
16  #  * Neither the name of the Willow Garage nor the names of its 
17  #    contributors may be used to endorse or promote products derived 
18  #    from this software without specific prior written permission. 
19  # 
20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
31  # POSSIBILITY OF SUCH DAMAGE. 
32   
33  #Tang Tiong Yew <tang.tiong.yew@gmail.com> 
34   
35  import rospy 
36  import math 
37  import copy 
38  import sensor_msgs.msg 
39  import PyKDL 
40   
41 -class EddiebotGyro(): 
42 -    def __init__(self): 
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   
56 -    def reconfigure(self, config, level):  
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   
62 -    def update_calibration(self, sensor_state): 
63          #check if we're not moving and update the calibration offset 
64          #to account for any calibration drift due to temperature 
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          #sign change 
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          #print orientation 
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          #sign change 
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          #print orientation 
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