oculusprime: arcmove_globalpath_follower.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
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 # allows odom + amcl to catch up
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) # (was 6) -- 0.21 minimum for pwm 255
00039 minlinear = 0.08 # was 0.05
00040 maxlinear = 0.5
00041 # maxarclinear = 0.75
00042 lastpath = 0  # refers to localpath
00043 goalpose = False
00044 goalseek = False
00045 
00046 # TODO: get these two values from java
00047 meterspersec = 0.33 # linear speed  
00048 radianspersec = 1.496 # 1.496 = 0.0857degrees per ms
00049 
00050 dpmthreshold = 1.2 # maximum to allow arcturn, radians per meter
00051 tfx = 0
00052 tfy = 0
00053 tfth = 0
00054 globalpathposenum = 20  
00055 listener = None
00056 
00057 
00058 def pathCallback(data): # local path
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] # get last pose in path
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         # do full rotation on pose estimate, to hone-in amcl (if not docked)
00107         rospy.sleep(0.5) # let amcl settle
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         # to prevent immediately rotating wrongly towards new goal direction 
00117         lastpath = rospy.get_time()
00118         goalpose = False
00119 
00120         # set goal angle
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 # sometimes globalpath still points at previoius goal
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] # get latest status
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         # global path targets
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         # local path targets    
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         # find arclength if any, and turn radians, depending on scenario
00163         arclength = 0
00164         goalrotate = False      
00165         if followpath and not ox==gpx and not oy==gpy and not initialturn: # normal arc move
00166                         
00167                 if abs(lpth-gpth) > dpmthreshold: # 90 degrees local path disparity, use global instead
00168                         dth = gpth
00169                         distance = gpdistance/2 # prevent oscillation, better than distance = 0
00170                         # distance = 0
00171                         # print("using global path")
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 # *should* work out to always be > 0
00184                 if not arclength == 0:
00185                         if abs(dth/arclength) > dpmthreshold: #  1.57:
00186                                 arclength = 0
00187                                 # print ("high dpm")
00188 
00189         elif goalpose:  # final goal rotate move
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: # initial turn move (always?)  
00199                 dth = gpth - oth # point to global path
00200 
00201         # determine angle delta for move TODO: cleanup
00202         if dth > math.pi:
00203                 dth = -math.pi*2 + dth
00204         elif dth < -math.pi:
00205                 dth = math.pi*2 + dth
00206                 
00207         # if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
00208         if abs(dth) > 1.57 and not goalrotate and not initialturn and waitonaboutface < 1: # was 2.094 120 deg
00209                 if goalDistance() > 0.9: # skip if close to goal
00210                         oculusprimesocket.clearIncoming()
00211                         oculusprimesocket.sendString("forward 0.25")
00212                         oculusprimesocket.waitForReplySearch("<state> direction stop")
00213                         waitonaboutface += 1 # only do this once
00214                         rospy.sleep(1.5)
00215                         nextmove = rospy.get_time() + listentime
00216                         # print("pausing...")
00217                         return
00218         waitonaboutface = 0
00219 
00220         initialturn = False
00221 
00222         if arclength > 0: # arcmove
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         # rotate only
00233 
00234         # force minimum
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         # below waits forever with new java stopbetweenmoves rotate code!! state never arrives
00244         # oculusprimesocket.waitForReplySearch("<state> direction stop")
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         # determine xy deltas for move
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                 # th = gth - tfth
00285 
00286         else: # does this ever get called? just use th = math.acos(dx/distance), same?
00287                 th = tth
00288         
00289         # determine angle delta for move
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         # force minimums        
00297         if distance > 0 and distance < minlinear:
00298                 distance = minlinear
00299                 
00300         if distance > maxlinear:
00301                 distance = maxlinear
00302 
00303         # supposed to reduce zig zagging  (was 0.3)
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         # if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
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 # only do this once
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                 # initialturn = False
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         # print ("goaldistance: "+str(distance)+", tfx: "+str(tfx)+", tfy: "+str(tfy))
00356         return distance
00357         
00358                         
00359 def cleanup():
00360         oculusprimesocket.sendString("log arcmove_globalpath_follower.py disconnecting")   
00361 
00362 
00363 # MAIN
00364 
00365 # rospy.init_node('dwa_base_controller', anonymous=False)
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) # blocking
00402                         else:
00403                                 # print ("using move() global path")
00404                                 move(odomx, odomy, odomth, gptargetx, gptargety, gptargetth, goalth) # blocking
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                 # goalpose = True
00414         
00415         rospy.sleep(0.01)
00416