oculusprime: remote_nav.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy, tf
00004 import os, struct, re
00005 from nav_msgs.msg import OccupancyGrid, Path
00006 from nav_msgs.msg import Odometry
00007 from geometry_msgs.msg import PoseWithCovarianceStamped
00008 import oculusprimesocket
00009 from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback
00010 from sensor_msgs.msg import LaserScan
00011 import actionlib
00012 from actionlib_msgs.msg import *
00013 from std_srvs.srv import Empty
00014 
00015 """
00016 sending info:
00017 rosmapinfo (origin, size, resolution) on callback
00018 rosamcl (x,y,th) on callback
00019     -also sending global path, scan data at this time, if exists
00020 roscurrentgoal  (x,y,th)    on callback
00021    
00022 change to:
00023 send amcl on callback, + latest odom << change to amcloffset only???
00024 roscurrentgoal, rosmapinfo unchanged
00025 
00026 send global path (without amcl offset), scan data, odom every 0.5 sec
00027 
00028 """
00029 
00030 
00031 lockfilepath = "/run/shm/map.raw.lock"
00032 lastodomupdate = 0
00033 odomx = 0
00034 odomy = 0
00035 odomth = 0
00036 globalpath = []
00037 scannum = 0
00038 scanpoints = []
00039 sendinfodelay = 1.0
00040 lastsendinfo = 0
00041 move_base = None
00042 goalseek = False
00043 xoffst = 0
00044 yoffst = 0
00045 thoffst = 0
00046 recoveryrotate = False
00047 goal = None
00048 turnspeed = 100
00049 secondspertwopi = 4.2 # calibration, automate? (do in java, faster)
00050 
00051 def mapcallBack(data):
00052         global lockfilepath
00053         lockfilepath = "/run/shm/map.raw.lock"
00054         framefilepath ="/run/shm/map.raw"
00055         
00056         if os.path.exists(lockfilepath):
00057                 return
00058         
00059         open(lockfilepath, 'w') # creates lockfile
00060          
00061         framefile = open(framefilepath, 'w')
00062         packeddata = struct.pack('%sb' %len(data.data), *data.data)
00063         framefile.write(packeddata)
00064         framefile.close()
00065 
00066         if os.path.exists(lockfilepath):
00067                 os.remove(lockfilepath)
00068 
00069         quaternion = ( data.info.origin.orientation.x, data.info.origin.orientation.y,
00070         data.info.origin.orientation.z, data.info.origin.orientation.w )
00071         th = tf.transformations.euler_from_quaternion(quaternion)[2]
00072                 
00073         # width height res originx originy originth updatetime  
00074         s = "state rosmapinfo "+str(data.info.width)+","+str(data.info.height)+","
00075         s += str(data.info.resolution)+","+str(data.info.origin.position.x)+","
00076         s += str(data.info.origin.position.y)+","+str(th)+","+str(rospy.get_time())
00077         
00078         oculusprimesocket.sendString(s)
00079         oculusprimesocket.sendString("state rosmapupdated true")
00080 
00081 def odomCallback(data):
00082          global odomx, odomy, odomth
00083          odomx = data.pose.pose.position.x
00084          odomy = data.pose.pose.position.y
00085          quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
00086          data.pose.pose.orientation.z, data.pose.pose.orientation.w )
00087          odomth = tf.transformations.euler_from_quaternion(quaternion)[2]
00088                 
00089 def amclPoseCallback(data):
00090         global odomx, odomy, odomth, goalseek, xoffst, yoffst, thoffst
00091         
00092         if not goalseek:
00093                 amclx = data.pose.pose.position.x
00094                 amcly = data.pose.pose.position.y
00095                 quaternion = ( data.pose.pose.orientation.x, data.pose.pose.orientation.y,
00096                 data.pose.pose.orientation.z, data.pose.pose.orientation.w )
00097                 amclth = tf.transformations.euler_from_quaternion(quaternion)[2]
00098 
00099                 xoffst = amclx - odomx
00100                 yoffst = amcly - odomy
00101                 thoffst = amclth - odomth
00102 
00103                 
00104 def feedbackCallback(d):
00105         global odomx, odomy, odomth, xoffst, yoffst, thoffst
00106         data = d.feedback.base_position.pose
00107         amclx = data.position.x
00108         amcly = data.position.y
00109         quaternion = ( data.orientation.x, data.orientation.y,
00110         data.orientation.z, data.orientation.w )
00111         amclth = tf.transformations.euler_from_quaternion(quaternion)[2]
00112 
00113         xoffst = amclx - odomx
00114         yoffst = amcly - odomy
00115         thoffst = amclth - odomth
00116         
00117 def goalCallback(d):
00118         data = d.goal.target_pose
00119         x = data.pose.position.x
00120         y = data.pose.position.y
00121         quaternion = ( data.pose.orientation.x, data.pose.orientation.y,
00122         data.pose.orientation.z, data.pose.orientation.w )
00123         th = tf.transformations.euler_from_quaternion(quaternion)[2]    
00124         oculusprimesocket.sendString("state roscurrentgoal "+str(x)+","+str(y)+","+str(th))
00125         oculusprimesocket.sendString("state delete rosgoalstatus")
00126         oculusprimesocket.sendString("messageclients new navigation goal received");
00127         
00128 def globalPathCallback(data):
00129         global globalpath
00130         globalpath = data.poses
00131         recoveryrotate = False
00132         
00133 def sendGlobalPath(path):
00134         s = "state rosglobalpath "
00135         step = 5 
00136         size = len(path)
00137         i = 0
00138 
00139         while i < size - step:
00140                 s = s + str(round(path[i].pose.position.x,2))+","
00141                 s = s + str(round(path[i].pose.position.y,2))+","
00142                 i += step
00143 
00144         s = s + str(round(path[size-1].pose.position.x,2))
00145         s = s + str(round(path[size-1].pose.position.y,2))
00146 
00147         oculusprimesocket.sendString(s)
00148 
00149 def publishinitialpose(str):
00150         s = str.split("_")
00151         x = float(s[0])
00152         y = float(s[1])
00153         th = float(s[2])
00154         
00155         pose = PoseWithCovarianceStamped()
00156         pose.header.stamp = rospy.Time.now()
00157         pose.header.frame_id = "map"
00158         
00159         pose.pose.pose.position.x = x
00160         pose.pose.pose.position.y = y
00161         pose.pose.pose.position.z = 0.0
00162         
00163         quat = tf.transformations.quaternion_from_euler(0, 0, th)
00164         pose.pose.pose.orientation.x = quat[0]
00165         pose.pose.pose.orientation.y = quat[1]
00166         pose.pose.pose.orientation.z = quat[2]
00167         pose.pose.pose.orientation.w = quat[3]
00168         pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
00169         
00170         initpose_pub.publish(pose)
00171                 
00172 def publishgoal(str):
00173         global move_base, goal
00174         
00175         s = str.split(",")
00176         x = float(s[0])
00177         y = float(s[1])
00178         th = float(s[2])
00179         
00180         goal = MoveBaseGoal()
00181         goal.target_pose.header.stamp = rospy.Time.now()
00182         goal.target_pose.header.frame_id = "map"
00183         
00184         goal.target_pose.pose.position.x = x
00185         goal.target_pose.pose.position.y = y
00186         goal.target_pose.pose.position.z = 0.0
00187         
00188         quat = tf.transformations.quaternion_from_euler(0, 0, th)
00189         goal.target_pose.pose.orientation.x = quat[0]
00190         goal.target_pose.pose.orientation.y = quat[1]
00191         goal.target_pose.pose.orientation.z = quat[2]
00192         goal.target_pose.pose.orientation.w = quat[3]
00193 
00194         move_base.send_goal(goal)
00195 
00196 def scanCallback(data):
00197         global scannum, scanpoints
00198         scannum += 1
00199         if scannum < 5:
00200                 return
00201         scannum=0
00202         scanpoints = data.ranges
00203 
00204 def sendScan():
00205         global scanpoints
00206         
00207         s = "state rosscan "
00208         
00209         step = 8 
00210         size = len(scanpoints)
00211         i = 0
00212         while i < size-step:
00213                 s += str(round(scanpoints[i],3))+","
00214                 i += step
00215         s += str(round(scanpoints[size-1],3))
00216         oculusprimesocket.sendString(s)
00217 
00218 def cleanup():
00219         oculusprimesocket.sendString("state delete roscurrentgoal")
00220         oculusprimesocket.sendString("state delete rosamcl")
00221         oculusprimesocket.sendString("state delete rosglobalpath")
00222         oculusprimesocket.sendString("state delete rosscan")
00223         oculusprimesocket.sendString("log remote_nav.py disconnecting")         
00224 
00225 def goalcancel():
00226         global goalseek, recoveryrotate
00227         
00228         move_base.cancel_goal()
00229         goalseek = False
00230         oculusprimesocket.sendString("messageclients cancel navigation goal")
00231         oculusprimesocket.sendString("state delete roscurrentgoal")
00232         oculusprimesocket.sendString("state delete rosgoalcancel")
00233         globalpath = []
00234         recoveryrotate = False
00235         
00236 # main
00237 
00238 oculusprimesocket.connect()     
00239 
00240 rospy.init_node('remote_nav', anonymous=False)
00241 
00242 if os.path.exists(lockfilepath):
00243         os.remove(lockfilepath)
00244         
00245 oculusprimesocket.sendString("log remote_nav.py connected")  
00246 oculusprimesocket.sendString("state delete roscurrentgoal")
00247 oculusprimesocket.sendString("state delete rosamcl")
00248 oculusprimesocket.sendString("state delete rosglobalpath")
00249 oculusprimesocket.sendString("state delete rosmapinfo")
00250 oculusprimesocket.sendString("state delete rosscan")
00251 oculusprimesocket.sendString("state delete rosgoalstatus")
00252         
00253 rospy.Subscriber("map", OccupancyGrid, mapcallBack)
00254 rospy.Subscriber("odom", Odometry, odomCallback)
00255 rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, amclPoseCallback)
00256 rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback)
00257 rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback) 
00258 rospy.Subscriber("scan", LaserScan, scanCallback)
00259 rospy.Subscriber("move_base/feedback", MoveBaseActionFeedback, feedbackCallback)
00260 initpose_pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
00261 rospy.on_shutdown(cleanup)
00262 
00263 move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
00264 move_base.wait_for_server()
00265 rospy.sleep(0.5)
00266 if not rospy.is_shutdown():
00267         oculusprimesocket.sendString("messageclients navigation ready") 
00268         oculusprimesocket.sendString("state navsystemstatus running") 
00269 
00270 lasttext = ""
00271 
00272 while not rospy.is_shutdown():
00273         s = oculusprimesocket.replyBufferSearch("<state> (rosinitialpose|rossetgoal|rosgoalcancel) ")
00274         if re.search("rosinitialpose", s):
00275                 oculusprimesocket.sendString("state delete rosinitialpose")
00276                 publishinitialpose(s.split()[2])
00277                 recoveryrotate = False
00278 
00279         elif re.search("rossetgoal", s):
00280                 oculusprimesocket.sendString("state delete rossetgoal")
00281                 globalpath = []
00282                 publishgoal(s.split()[2])
00283                 goalseek = True
00284                 recoveryrotate = False
00285         
00286         elif re.search("rosgoalcancel true", s):
00287                 goalcancel()
00288                 
00289         t = rospy.get_time()
00290         if t - lastsendinfo > sendinfodelay:
00291                 lastsendinfo = t
00292 
00293                 if len(scanpoints) > 0:
00294                         sendScan()
00295                         
00296                 # if goalseek and not xoffst == None:
00297                 s = "state rosamcl "
00298                 s += str(round(xoffst, 3))+","+str(round(yoffst,3))+","+str(round(thoffst,3))+","
00299                 s += str(round(odomx,3))+","+str(round(odomy,3))+","+str(round(odomth,3))
00300 
00301                 oculusprimesocket.sendString(s)
00302                 
00303                 if len(globalpath) > 0:
00304                         sendGlobalPath(globalpath)
00305                         
00306         if goalseek: 
00307                 state = move_base.get_state()
00308                 if state == GoalStatus.SUCCEEDED: # error if not seeking goal
00309                         oculusprimesocket.sendString("messageclients navigation goal reached")
00310                         oculusprimesocket.sendString("state rosgoalstatus succeeded") # having this below the one below may have caused null pointer
00311                         oculusprimesocket.sendString("state delete roscurrentgoal")
00312                         goalseek = False
00313                 elif state == GoalStatus.ABORTED: 
00314                         if not recoveryrotate:
00315                                 recoveryrotate = True
00316                                 
00317                                 #### recovery routine
00318 
00319                                 oculusprimesocket.sendString("messageclients recovery rotation")
00320                                 oculusprimesocket.clearIncoming()
00321                                 
00322                                 # cancel goal, clear costmaps, generally reset as much as possible
00323                                 move_base.cancel_goal()
00324                                 rospy.wait_for_service('/move_base/clear_costmaps')
00325                                 rospy.ServiceProxy('/move_base/clear_costmaps', Empty)()
00326 
00327                                 # wait for cpu
00328                                 rospy.sleep(2) 
00329                                 oculusprimesocket.sendString("waitforcpu")
00330                                 oculusprimesocket.waitForReplySearch("<state> waitingforcpu false")
00331                                 
00332                                 # check if cancelled by user while waiting
00333                                 oculusprimesocket.sendString("state rosgoalcancel") 
00334                                 s = oculusprimesocket.waitForReplySearch("<state> rosgoalcancel") 
00335                                 if re.search("rosgoalcancel true", s):
00336                                         goalcancel()
00337                                         continue
00338                         
00339                                 # resend pose, full rotate
00340                                 publishinitialpose(str(xoffst+odomx)+"_"+str(yoffst+odomy)+"_"+str(thoffst+odomth))
00341                                 # oculusprimesocket.sendString("right 360")
00342                                 oculusprimesocket.waitForReplySearch("<state> direction stop")
00343 
00344                                 # wait for cpu
00345                                 rospy.sleep(2) 
00346                                 oculusprimesocket.sendString("waitforcpu")
00347                                 oculusprimesocket.waitForReplySearch("<state> waitingforcpu false")
00348 
00349                                 # check if cancelled by user while waiting
00350                                 oculusprimesocket.sendString("state rosgoalcancel") 
00351                                 s = oculusprimesocket.waitForReplySearch("<state> rosgoalcancel") 
00352                                 if re.search("rosgoalcancel true", s):
00353                                         goalcancel()
00354                                         continue
00355 
00356                                 move_base.send_goal(goal) # try once more
00357 
00358                                 #### end of recovery routine
00359 
00360                                 
00361                         else:
00362                                 oculusprimesocket.sendString("messageclients navigation goal ABORTED")
00363                                 oculusprimesocket.sendString("state rosgoalstatus aborted")
00364                                 oculusprimesocket.sendString("state delete roscurrentgoal")
00365                                 goalseek = False
00366                 
00367                 
00368         rospy.sleep(0.01) # non busy wait
00369