00001
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
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')
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
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
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
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:
00309 oculusprimesocket.sendString("messageclients navigation goal reached")
00310 oculusprimesocket.sendString("state rosgoalstatus succeeded")
00311 oculusprimesocket.sendString("state delete roscurrentgoal")
00312 goalseek = False
00313 elif state == GoalStatus.ABORTED:
00314 if not recoveryrotate:
00315 recoveryrotate = True
00316
00317
00318
00319 oculusprimesocket.sendString("messageclients recovery rotation")
00320 oculusprimesocket.clearIncoming()
00321
00322
00323 move_base.cancel_goal()
00324 rospy.wait_for_service('/move_base/clear_costmaps')
00325 rospy.ServiceProxy('/move_base/clear_costmaps', Empty)()
00326
00327
00328 rospy.sleep(2)
00329 oculusprimesocket.sendString("waitforcpu")
00330 oculusprimesocket.waitForReplySearch("<state> waitingforcpu false")
00331
00332
00333 oculusprimesocket.sendString("state rosgoalcancel")
00334 s = oculusprimesocket.waitForReplySearch("<state> rosgoalcancel")
00335 if re.search("rosgoalcancel true", s):
00336 goalcancel()
00337 continue
00338
00339
00340 publishinitialpose(str(xoffst+odomx)+"_"+str(yoffst+odomy)+"_"+str(thoffst+odomth))
00341
00342 oculusprimesocket.waitForReplySearch("<state> direction stop")
00343
00344
00345 rospy.sleep(2)
00346 oculusprimesocket.sendString("waitforcpu")
00347 oculusprimesocket.waitForReplySearch("<state> waitingforcpu false")
00348
00349
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)
00357
00358
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)
00369