00001
00002
00003 """
00004 on any new /initialpose, do full rotation, then delay (to hone in amcl)
00005
00006 requires dwa_base_controller, global path updated continuously as bot moves
00007
00008 """
00009
00010
00011 import rospy, tf
00012 import oculusprimesocket
00013 from nav_msgs.msg import Odometry
00014 import math, re
00015 from nav_msgs.msg import Path
00016 from geometry_msgs.msg import PoseWithCovarianceStamped
00017 from actionlib_msgs.msg import GoalStatusArray
00018 from move_base_msgs.msg import MoveBaseActionGoal
00019
00020 listentime = 0.6
00021 nextmove = 0
00022 odomx = 0
00023 odomy = 0
00024 odomth = 0
00025 gptargetx = 0
00026 gptargety = 0
00027 gptargetth = 0
00028 lptargetx = 0
00029 lptargety = 0
00030 lptargetth = 0
00031 followpath = False
00032 pathid = None
00033 goalx = 0
00034 goaly = 0
00035 goalth = 0
00036 initialturn = False
00037 waitonaboutface = 0
00038 minturn = math.radians(8)
00039 minlinear = 0.08
00040 maxlinear = 0.5
00041
00042 lastpath = 0
00043 goalpose = False
00044 goalseek = False
00045
00046
00047 meterspersec = 0.33
00048 radianspersec = 1.496
00049
00050 dpmthreshold = 1.2
00051 tfx = 0
00052 tfy = 0
00053 tfth = 0
00054 globalpathposenum = 20
00055 listener = None
00056
00057
00058 def pathCallback(data):
00059 global goalpose, lastpath, followpath
00060 global lptargetx, lptargety
00061
00062 lastpath = rospy.get_time()
00063 goalpose = False
00064
00065 p = data.poses[len(data.poses)-1]
00066 lptargetx = p.pose.position.x
00067 lptargety = p.pose.position.y
00068 quaternion = ( p.pose.orientation.x, p.pose.orientation.y,
00069 p.pose.orientation.z, p.pose.orientation.w )
00070 lptargetth = tf.transformations.euler_from_quaternion(quaternion)[2]
00071
00072 def globalPathCallback(data):
00073 global gptargetx, gptargety, gptargetth, followpath, pathid
00074
00075 n = len(data.poses)
00076 if n < 5:
00077 return
00078
00079 if n-1 < globalpathposenum:
00080 p = data.poses[n-1]
00081 else:
00082 p = data.poses[globalpathposenum]
00083
00084 gptargetx = p.pose.position.x
00085 gptargety = p.pose.position.y
00086 quaternion = ( p.pose.orientation.x, p.pose.orientation.y,
00087 p.pose.orientation.z, p.pose.orientation.w )
00088 gptargetth = tf.transformations.euler_from_quaternion(quaternion)[2]
00089
00090 pathid = data.header.seq
00091 followpath = True
00092
00093
00094 def odomCallback(data):
00095 global odomx, odomy, odomth
00096 odomx = data.pose.pose.position.x
00097 odomy = data.pose.pose.position.y
00098 quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
00099 data.pose.pose.orientation.z, data.pose.pose.orientation.w )
00100 odomth = tf.transformations.euler_from_quaternion(quaternion)[2]
00101
00102
00103 def intialPoseCallback(data):
00104 if data.pose.pose.position.x == 0 and data.pose.pose.position.y == 0:
00105 return
00106
00107 rospy.sleep(0.5)
00108 oculusprimesocket.clearIncoming()
00109 oculusprimesocket.sendString("right 360")
00110 oculusprimesocket.waitForReplySearch("<state> direction stop")
00111
00112
00113 def goalCallback(d):
00114 global goalx, goaly, goalth, goalpose, lastpath, initialturn, followpath, nextmove
00115
00116
00117 lastpath = rospy.get_time()
00118 goalpose = False
00119
00120
00121 data = d.goal.target_pose
00122 goalx = data.pose.position.x
00123 goaly = data.pose.position.y
00124 quaternion = ( data.pose.orientation.x, data.pose.orientation.y,
00125 data.pose.orientation.z, data.pose.orientation.w )
00126 goalth = tf.transformations.euler_from_quaternion(quaternion)[2]
00127 initialturn = True
00128 followpath = False
00129 nextmove = lastpath + 2
00130
00131 def goalStatusCallback(data):
00132 global goalseek
00133 goalseek = False
00134 if len(data.status_list) == 0:
00135 return
00136 status = data.status_list[len(data.status_list)-1]
00137 if status.status == 1:
00138 goalseek = True
00139
00140 def arcmove(ox, oy, oth, gpx, gpy, gpth, gth, lpx, lpy, lpth):
00141
00142 global initialturn, waitonaboutface, nextmove
00143
00144
00145 gpdx = gpx - ox
00146 gpdy = gpy - oy
00147 gpdistance = math.sqrt( pow(gpdx,2) + pow(gpdy,2) )
00148 if not gpdistance == 0:
00149 gpth = math.acos(gpdx/gpdistance)
00150 if gpdy <0:
00151 gpth = -gpth
00152
00153
00154 lpdx = lpx - ox
00155 lpdy = lpy - oy
00156 lpdistance = math.sqrt( pow(lpdx,2) + pow(lpdy,2) )
00157 if not lpdistance == 0:
00158 lpth = math.acos(lpdx/lpdistance)
00159 if lpdy <0:
00160 lpth = -lpth
00161
00162
00163 arclength = 0
00164 goalrotate = False
00165 if followpath and not ox==gpx and not oy==gpy and not initialturn:
00166
00167 if abs(lpth-gpth) > dpmthreshold:
00168 dth = gpth
00169 distance = gpdistance/2
00170
00171
00172 else:
00173 dth = lpth
00174 distance = lpdistance
00175
00176 dth = dth - oth
00177 if dth > math.pi:
00178 dth = -math.pi*2 + dth
00179 elif dth < -math.pi:
00180 dth = math.pi*2 + dth
00181
00182 radius = (distance/2)/math.sin(dth/2)
00183 arclength = radius * dth
00184 if not arclength == 0:
00185 if abs(dth/arclength) > dpmthreshold:
00186 arclength = 0
00187
00188
00189 elif goalpose:
00190 try:
00191 (trans,rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0))
00192 quaternion = (rot[0], rot[1], rot[2], rot[3])
00193 dth = (gth - tf.transformations.euler_from_quaternion(quaternion)[2]) - oth
00194 goalrotate = True
00195 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00196 dth = lpth
00197
00198 else:
00199 dth = gpth - oth
00200
00201
00202 if dth > math.pi:
00203 dth = -math.pi*2 + dth
00204 elif dth < -math.pi:
00205 dth = math.pi*2 + dth
00206
00207
00208 if abs(dth) > 1.57 and not goalrotate and not initialturn and waitonaboutface < 1:
00209 if goalDistance() > 0.9:
00210 oculusprimesocket.clearIncoming()
00211 oculusprimesocket.sendString("forward 0.25")
00212 oculusprimesocket.waitForReplySearch("<state> direction stop")
00213 waitonaboutface += 1
00214 rospy.sleep(1.5)
00215 nextmove = rospy.get_time() + listentime
00216
00217 return
00218 waitonaboutface = 0
00219
00220 initialturn = False
00221
00222 if arclength > 0:
00223 if arclength < minlinear:
00224 arclength = minlinear
00225
00226 oculusprimesocket.sendString("arcmove " + str(arclength) + " " + str(int(math.degrees(dth))) )
00227 rospy.sleep(arclength/0.35)
00228 nextmove = rospy.get_time()
00229 return
00230
00231
00232
00233
00234
00235 if dth > 0 and dth < minturn:
00236 dth = minturn
00237 elif dth < 0 and dth > -minturn:
00238 dth = -minturn
00239
00240 oculusprimesocket.clearIncoming()
00241
00242 oculusprimesocket.sendString("move stop")
00243
00244
00245 rospy.sleep(0.5)
00246
00247 if dth > 0:
00248 oculusprimesocket.sendString("left " + str(int(math.degrees(dth))) )
00249 oculusprimesocket.waitForReplySearch("<state> direction stop")
00250 elif dth < 0:
00251 oculusprimesocket.sendString("right " +str(int(math.degrees(-dth))) )
00252 oculusprimesocket.waitForReplySearch("<state> direction stop")
00253
00254 nextmove = rospy.get_time() + listentime
00255
00256
00257 def move(ox, oy, oth, tx, ty, tth, gth):
00258 global followpath, initialturn, waitonaboutface, nextmove
00259
00260 currentpathid = pathid
00261
00262
00263 distance = 0
00264 if followpath:
00265 dx = tx - ox
00266 dy = ty - oy
00267 distance = math.sqrt( pow(dx,2) + pow(dy,2) )
00268
00269 goalrotate = False
00270 if distance > 0:
00271 th = math.acos(dx/distance)
00272 if dy <0:
00273 th = -th
00274
00275 elif goalpose:
00276 try:
00277 (trans,rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0))
00278 quaternion = (rot[0], rot[1], rot[2], rot[3])
00279 th = gth - tf.transformations.euler_from_quaternion(quaternion)[2]
00280 goalrotate = True
00281 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00282 th = tth
00283
00284
00285
00286 else:
00287 th = tth
00288
00289
00290 dth = th - oth
00291 if dth > math.pi:
00292 dth = -math.pi*2 + dth
00293 elif dth < -math.pi:
00294 dth = math.pi*2 + dth
00295
00296
00297 if distance > 0 and distance < minlinear:
00298 distance = minlinear
00299
00300 if distance > maxlinear:
00301 distance = maxlinear
00302
00303
00304 if dth < minturn*0.5 and dth > -minturn*0.5:
00305 dth = 0
00306 elif dth >= minturn*0.5 and dth < minturn:
00307 dth = minturn
00308 elif dth <= -minturn*0.5 and dth > -minturn:
00309 dth = -minturn
00310
00311 oculusprimesocket.clearIncoming()
00312
00313
00314 if abs(dth) > 1.57 and not goalrotate and not initialturn and waitonaboutface < 1:
00315 if goalDistance() > 0.9:
00316 oculusprimesocket.sendString("forward 0.26")
00317 oculusprimesocket.waitForReplySearch("<state> direction stop")
00318 waitonaboutface += 1
00319 rospy.sleep(1.5)
00320 nextmove = rospy.get_time() + listentime
00321 return
00322
00323 waitonaboutface = 0
00324 initialturn = False
00325
00326 if not pathid == currentpathid:
00327 nextmove = rospy.get_time() + listentime
00328 return
00329
00330 if dth > 0:
00331 oculusprimesocket.sendString("left " + str(int(math.degrees(dth))) )
00332 oculusprimesocket.waitForReplySearch("<state> direction stop")
00333 elif dth < 0:
00334 oculusprimesocket.sendString("right " +str(int(math.degrees(-dth))) )
00335 oculusprimesocket.waitForReplySearch("<state> direction stop")
00336
00337 if distance > 0:
00338 oculusprimesocket.sendString("forward "+str(distance))
00339 rospy.sleep(distance/meterspersec)
00340
00341
00342 nextmove = rospy.get_time() + listentime
00343
00344
00345 def goalDistance():
00346
00347 try:
00348 (trans,rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0))
00349 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00350 return 99
00351
00352 gdx = goalx - trans[0]
00353 gdy = goaly - trans[1]
00354 distance = math.sqrt( pow(gdx,2) + pow(gdy,2) )
00355
00356 return distance
00357
00358
00359 def cleanup():
00360 oculusprimesocket.sendString("log arcmove_globalpath_follower.py disconnecting")
00361
00362
00363
00364
00365
00366 rospy.init_node('arcmove_globalpath_follower', anonymous=False)
00367 listener = tf.TransformListener()
00368 oculusprimesocket.connect()
00369 rospy.on_shutdown(cleanup)
00370
00371 rospy.Subscriber("odom", Odometry, odomCallback)
00372 rospy.Subscriber("move_base/DWAPlannerROS/local_plan", Path, pathCallback)
00373 rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback)
00374 rospy.Subscriber("move_base/status", GoalStatusArray, goalStatusCallback)
00375 rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback)
00376 rospy.Subscriber("initialpose", PoseWithCovarianceStamped, intialPoseCallback)
00377
00378 oculusprimesocket.sendString("log arcmove_globalpath_follower.py connected")
00379 oculusprimesocket.sendString("readsetting usearcmoves")
00380 s = oculusprimesocket.waitForReplySearch("setting usearcmoves")
00381 if re.search("true", s):
00382 oculusprimesocket.sendString("state rosarcmove true")
00383 else:
00384 oculusprimesocket.sendString("state rosarcmove false")
00385
00386
00387 while not rospy.is_shutdown():
00388 t = rospy.get_time()
00389
00390 if t >= nextmove:
00391 if goalseek and (followpath or goalpose):
00392
00393 oculusprimesocket.sendString("state rosarcmove")
00394 s = oculusprimesocket.waitForReplySearch("<state> rosarcmove")
00395 if re.search("true", s):
00396 rosarcmove = True
00397 else:
00398 rosarcmove = False
00399
00400 if rosarcmove and goalDistance() > 0.9:
00401 arcmove(odomx, odomy, odomth, gptargetx, gptargety, gptargetth, goalth, lptargetx, lptargety, lptargetth)
00402 else:
00403
00404 move(odomx, odomy, odomth, gptargetx, gptargety, gptargetth, goalth)
00405
00406 followpath = False
00407
00408 if t - lastpath > 3 and goalseek:
00409 if goalDistance() <= 0.9:
00410 goalpose = True
00411 else:
00412 rospy.sleep(0.5)
00413
00414
00415 rospy.sleep(0.01)
00416