Go to the documentation of this file.00001
00002
00003 import rospy, tf
00004 import os, struct, re
00005 from nav_msgs.msg import OccupancyGrid
00006 from sensor_msgs.msg import LaserScan
00007 import oculusprimesocket
00008
00009 """
00010
00011
00012 """
00013
00014 lockfilepath = "/run/shm/map.raw.lock"
00015 scannum = 0
00016 scanpoints = []
00017 sendinfodelay = 1.0
00018 lastsendinfo = 0
00019
00020 def mapcallBack(data):
00021 global lockfilepath
00022 lockfilepath = "/run/shm/map.raw.lock"
00023 framefilepath ="/run/shm/map.raw"
00024
00025 if os.path.exists(lockfilepath):
00026 return
00027
00028 open(lockfilepath, 'w')
00029
00030 framefile = open(framefilepath, 'w')
00031 packeddata = struct.pack('%sb' %len(data.data), *data.data)
00032 framefile.write(packeddata)
00033 framefile.close()
00034
00035 if os.path.exists(lockfilepath):
00036 os.remove(lockfilepath)
00037
00038 quaternion = ( data.info.origin.orientation.x, data.info.origin.orientation.y,
00039 data.info.origin.orientation.z, data.info.origin.orientation.w )
00040 th = tf.transformations.euler_from_quaternion(quaternion)[2]
00041
00042
00043 s = "state rosmapinfo "+str(data.info.width)+","+str(data.info.height)+","
00044 s += str(data.info.resolution)+","+str(data.info.origin.position.x)+","
00045 s += str(data.info.origin.position.y)+","+str(th)+","+str(rospy.get_time())
00046
00047 oculusprimesocket.sendString(s)
00048 oculusprimesocket.sendString("state rosmapupdated true")
00049
00050 def scanCallback(data):
00051 global firstscan
00052 firstscan.unregister()
00053 oculusprimesocket.sendString("state navsystemstatus mapping")
00054
00055
00056
00057
00058
00059
00060
00061
00062 def sendScan():
00063 global scanpoints
00064
00065 s = "state rosscan "
00066
00067 step = 8
00068 size = len(scanpoints)
00069 i = 0
00070 while i < size-step:
00071 s += str(round(scanpoints[i],3))+","
00072 i += step
00073 s += str(round(scanpoints[size-1],3))
00074 oculusprimesocket.sendString(s)
00075
00076
00077
00078 oculusprimesocket.connect()
00079
00080 oculusprimesocket.sendString("state delete roscurrentgoal")
00081 oculusprimesocket.sendString("state delete rosamcl")
00082 oculusprimesocket.sendString("state delete rosglobalpath")
00083 oculusprimesocket.sendString("state delete rosmapinfo")
00084 oculusprimesocket.sendString("state delete rosscan")
00085 oculusprimesocket.sendString("state delete rosmapupdated")
00086
00087
00088
00089
00090
00091 rospy.init_node('map_remote', anonymous=False)
00092
00093 if os.path.exists(lockfilepath):
00094 os.remove(lockfilepath)
00095
00096 rospy.Subscriber("map", OccupancyGrid, mapcallBack)
00097 firstscan = rospy.Subscriber("scan", LaserScan, scanCallback)
00098
00099
00100
00101 while not rospy.is_shutdown():
00102
00103 t = rospy.get_time()
00104 if t - lastsendinfo > sendinfodelay:
00105 lastsendinfo = t
00106
00107 if len(scanpoints) > 0:
00108 sendScan()
00109
00110 rospy.sleep(0.01)
00111