Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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
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
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()
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
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
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
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():
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:
00217 if angle == 0:
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
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
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
00261 self._error_scan_angle_publisher.publish(error_scan_angle)
00262
00263 def scan_callback(self, msg):
00264 with self.lock:
00265
00266 self._scan_angle = msg.scan_angle
00267 self._scan_time = msg.header.stamp
00268
00269