drift_estimation.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #       
00003 # License: BSD
00004 #   https://raw.github.com/yujinrobot/kobuki/hydro-devel/kobuki_testsuite/LICENSE
00005 #
00006 ##############################################################################
00007 # Imports
00008 ##############################################################################
00009 
00010 import math
00011 import threading
00012 import PyKDL
00013 import rospy
00014 from sensor_msgs.msg import LaserScan, Imu
00015 from geometry_msgs.msg import Twist
00016 from kobuki_testsuite.msg import ScanAngle
00017 
00018 ##############################################################################
00019 # Utilities
00020 ##############################################################################
00021 
00022 def quat_to_angle(quat):
00023     rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
00024     return rot.GetRPY()[2]
00025 
00026 def wrap_angle(angle):
00027     if angle <= math.pi and angle >= -math.pi:
00028         return angle
00029     elif angle < 0.0:
00030         return math.fmod(angle-math.pi,2.0*math.pi)+math.pi
00031     else:
00032         return math.fmod(angle+math.pi,2.0*math.pi)-math.pi;
00033                 
00034 ##############################################################################
00035 # Classes
00036 ##############################################################################
00037 
00038 class ScanToAngle(object):
00039     def __init__(self, scan_topic, scan_angle_topic):
00040         self.min_angle = -0.3
00041         self.max_angle = 0.3
00042         self.lock = threading.Lock() # make sure we don't publish if the publisher is not there
00043         self._laser_scan_angle_publisher = rospy.Publisher(scan_angle_topic, ScanAngle)
00044         self.scan_subscriber = rospy.Subscriber(scan_topic, LaserScan, self.scan_callback)
00045 
00046     def init(self, min_angle=-0.3, max_angle=0.3):
00047         self.min_angle = min_angle
00048         self.max_angle = max_angle
00049 
00050     def shutdown(self):
00051         print "Killing off scan angle publisher"
00052         if not rospy.is_shutdown():
00053             with self.lock:
00054                 self.scan_subscriber.unregister()
00055                 self.scan_subscriber = None
00056                 self._laser_scan_angle_publisher.unregister()
00057                 del self._laser_scan_angle_publisher
00058                 self._laser_scan_angle_publisher = None
00059 
00060     def scan_callback(self, msg):
00061         angle = msg.angle_min
00062         d_angle = msg.angle_increment
00063         sum_x = 0
00064         sum_y = 0
00065         sum_xx = 0
00066         sum_xy = 0
00067         num = 0
00068         for r in msg.ranges:
00069             if angle > self.min_angle and angle < self.max_angle and r < msg.range_max:
00070                 x = math.sin(angle) * r
00071                 y = math.cos(angle) * r
00072                 sum_x += x
00073                 sum_y += y
00074                 sum_xx += x*x
00075                 sum_xy += x*y
00076                 num += 1
00077             angle += d_angle
00078         if num > 0:
00079             denominator = num*sum_xx-sum_x*sum_x
00080             if denominator != 0:
00081                 angle=math.atan2((-sum_x*sum_y+num*sum_xy)/(denominator), 1)
00082                 #print("Scan Angle: %s"%str(angle))
00083                 relay = ScanAngle()
00084                 relay.header = msg.header 
00085                 relay.scan_angle = angle
00086                 with self.lock:
00087                     if self._laser_scan_angle_publisher:
00088                         self._laser_scan_angle_publisher.publish(relay)
00089         else:
00090             rospy.logerr("Please point me at a wall.")
00091 
00092 class DriftEstimation(object):
00093     def __init__(self, laser_scan_angle_topic, gyro_scan_angle_topic, error_scan_angle_topic, cmd_vel_topic, gyro_topic):
00094         self.lock = threading.Lock()
00095         
00096         self._gyro_scan_angle_publisher = rospy.Publisher(gyro_scan_angle_topic, ScanAngle)
00097         self._laser_scan_angle_subscriber = rospy.Subscriber(laser_scan_angle_topic, ScanAngle, self.scan_callback)
00098         self._error_scan_angle_publisher = rospy.Publisher(error_scan_angle_topic, ScanAngle)
00099         self.gyro_subscriber  = rospy.Subscriber(gyro_topic, Imu, self.gyro_callback)
00100         self.cmd_vel_publisher = rospy.Publisher(cmd_vel_topic, Twist)
00101 
00102         self.rate = rospy.Rate(30)
00103 
00104         self._gyro_angle = 0
00105         self._gyro_time = rospy.Time.now()
00106         self._scan_angle = 0
00107         self._scan_time = rospy.Time.now()
00108         
00109         ##############################
00110         # Parameters
00111         ##############################
00112         self._inital_wall_angle = 0.1
00113         self._max_angle = 0.4
00114         self._abs_yaw_rate = 0.3
00115         self._epsilon = 0.5 # radians value above which increasing difference in scan and gyro angle will be ignored
00116 
00117         self._centred_gyro_angle = None
00118         self._initial_gyro_offset = None
00119 
00120         self._stop = False
00121         self._running = False
00122 
00123     def init(self, yaw_rate):
00124         self._abs_yaw_rate = yaw_rate
00125     
00126     def shutdown(self):
00127         self.stop()
00128         while self._running:
00129             self.rate.sleep()
00130         if not rospy.is_shutdown():
00131             print "Shutting down drift estimation"
00132             self._gyro_scan_angle_publisher.unregister()
00133             self._gyro_scan_angle_publisher = None
00134             self._laser_scan_angle_subscriber.unregister()
00135             self._laser_scan_angle_subscriber = None
00136             self._error_scan_angle_publisher.unregister()
00137             self._error_scan_angle_publisher = None
00138             self.gyro_subscriber.unregister()
00139             self.gyro_subscriber = None
00140             self.cmd_vel_publisher.unregister()
00141             self.cmd_vel_publisher = None
00142         
00143     def stop(self):
00144         self._stop = True
00145     
00146     def execute(self):
00147         '''
00148           Drop this into threading.Thread or QThread for execution
00149         '''
00150         if self._running:
00151             rospy.logerr("Kobuki TestSuite: already executing, ignoring the request")
00152             return
00153         self._stop = False
00154         self._running = True
00155         if not self.align(): # centred_gyro_angle set in here
00156             rospy.loginfo("Kobuki Testsuite: could not align, please point me at a wall.")
00157             self._initial_gyro_offset = None
00158             self._running = False
00159             return
00160         with self.lock:
00161             if not self._initial_gyro_offset:
00162                 self._initial_gyro_offset = self._gyro_angle - self._scan_angle
00163                 print("Kobuki Testsuite: initial centre [%s]"%self._centred_gyro_angle)
00164                 print("Kobuki Testsuite: initial offset [%s]"%self._initial_gyro_offset)
00165         last_gyro_time = rospy.get_rostime()
00166         last_scan_time = rospy.get_rostime()
00167         yaw_rate_cmd = self._abs_yaw_rate
00168         turn_count = 0
00169         gyro_timeout_count = 0
00170         while not self._stop and not rospy.is_shutdown():
00171             with self.lock:
00172                 gyro_time = self._gyro_time
00173                 scan_time = self._scan_time
00174                 gyro_angle = self._gyro_angle
00175                 scan_angle = self._scan_angle
00176             if gyro_time > last_gyro_time:
00177                 last_gyro_time = gyro_time
00178                 if wrap_angle(gyro_angle - self._centred_gyro_angle) > self._max_angle:
00179                     if yaw_rate_cmd > 0: 
00180                         turn_count = turn_count + 1
00181                     yaw_rate_cmd = -self._abs_yaw_rate
00182                 elif wrap_angle(gyro_angle - self._centred_gyro_angle) < -self._max_angle:
00183                     yaw_rate_cmd = self._abs_yaw_rate
00184                 else:
00185                     yaw_rate_cmd = cmp(yaw_rate_cmd,0)*self._abs_yaw_rate
00186                 cmd = Twist()
00187                 cmd.angular.z = yaw_rate_cmd
00188                 self.cmd_vel_publisher.publish(cmd)
00189                 if scan_time > last_scan_time:
00190                     rospy.loginfo("Kobuki Testsuite: gyro, laser angle comparison [%s,%s]"%(gyro_angle - self._initial_gyro_offset, scan_angle))
00191                     last_scan_time = scan_time
00192             else:
00193                 gyro_timeout_count = gyro_timeout_count + 1
00194                 if gyro_timeout_count > 50:
00195                     rospy.logerr("Kobuki Testsuite: no gyro data for a long time.")
00196                     break
00197             if turn_count > 4:
00198                 rospy.loginfo("Kobuki Testsuite: aligning.....")
00199                 self.align()
00200                 turn_count = 0
00201             rospy.sleep(0.05)
00202         if not rospy.is_shutdown():
00203             cmd = Twist()
00204             cmd.angular.z = 0.0
00205             self.cmd_vel_publisher.publish(cmd)
00206         self._initial_gyro_offset = None
00207         self._running = False
00208 
00209     def align(self):
00210         with self.lock:
00211             angle = self._scan_angle
00212         no_data_count = 0
00213         count = 0
00214         epsilon = 0.05
00215         cmd = Twist()
00216         while True: #self._inital_wall_angle:
00217             if angle == 0: # magic number, means we have no data yet
00218                 no_data_count += 1
00219                 if no_data_count == 40:
00220                     return False
00221             if self._stop or rospy.is_shutdown():
00222                 return False
00223             elif count > 20:
00224                 with self.lock:
00225                     self._centred_gyro_angle = self._gyro_angle
00226                 cmd.angular.z = 0.0
00227                 self.cmd_vel_publisher.publish(cmd)
00228                 return True
00229             if math.fabs(angle) < 0.05:
00230                 count = count + 1
00231             # The work
00232             if angle > 0:
00233                 cmd.angular.z = -0.2
00234             else:
00235                 cmd.angular.z = 0.2
00236             self.cmd_vel_publisher.publish(cmd)
00237             rospy.sleep(0.05)
00238             with self.lock:
00239                 angle = self._scan_angle
00240         print "end of align"
00241 
00242     ##########################################################################
00243     # Ros Callbacks
00244     ##########################################################################
00245     
00246     def gyro_callback(self, msg):
00247         with self.lock:
00248             angle = quat_to_angle(msg.orientation)
00249             self._gyro_angle = angle
00250             self._gyro_time = msg.header.stamp
00251             if self._initial_gyro_offset:
00252                 gyro_scan_angle = ScanAngle()
00253                 gyro_scan_angle.header = msg.header 
00254                 gyro_scan_angle.scan_angle = angle - self._initial_gyro_offset 
00255                 self._gyro_scan_angle_publisher.publish(gyro_scan_angle)
00256                 if self._running:
00257                     error_scan_angle = ScanAngle()
00258                     error_scan_angle.header = msg.header 
00259                     error_scan_angle.scan_angle = math.fabs(angle - self._initial_gyro_offset - self._scan_angle)
00260                     #if error_scan_angle.scan_angle < self._epsilon: # don't spam with errors greater than what we're looking for
00261                     self._error_scan_angle_publisher.publish(error_scan_angle)
00262 
00263     def scan_callback(self, msg):
00264         with self.lock:
00265             #rospy.loginfo("Kobuki Testsuite: scan angle [%s]"%msg.scan_angle)
00266             self._scan_angle = msg.scan_angle
00267             self._scan_time = msg.header.stamp
00268 
00269 


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Wed Sep 16 2015 04:35:48