$search
00001 #!/usr/bin/env python 00002 00003 import copy 00004 import math 00005 import rospy 00006 import rosservice 00007 import Image 00008 import cStringIO 00009 import unavail 00010 import threading 00011 00012 import tf 00013 import tf.transformations 00014 00015 from geometry_msgs.msg import PoseWithCovariance, PoseWithCovarianceStamped, PoseStamped, Pose, Quaternion, Point 00016 from nav_msgs.msg import MapMetaData, OccupancyGrid 00017 from numpy import float64 00018 00019 from simplify import Simplifier 00020 import rosweb 00021 00022 _lock = threading.Lock() 00023 00024 _map_cache = {} 00025 _scale_cache = {} 00026 _tile_cache = {} 00027 00028 def config_plugin(context): 00029 global goal_publisher, pose_publisher 00030 goal_publisher = rospy.Publisher('/move_base_simple/goal', PoseStamped) 00031 pose_publisher = rospy.Publisher('/initialpose', PoseWithCovarianceStamped) 00032 00033 context.register_handler("/map", map_tiler_handler) 00034 context.register_subtopic("/move_base_node/NavfnROS/plan:simplified", Simplifier) 00035 00036 def trans(x): 00037 if x < 0: 00038 return chr(x + 256) 00039 else: 00040 return chr(x) 00041 00042 def get_map(service_name): 00043 map_key = service_name 00044 if not _map_cache.has_key(map_key): 00045 service_class = rosservice.get_service_class_by_name('/' + service_name) 00046 service_proxy = rospy.ServiceProxy(service_name, service_class) 00047 resp = service_proxy() 00048 size = (resp.map.info.width, resp.map.info.height) 00049 s = "".join([trans(x) for x in resp.map.data]) 00050 im = Image.frombuffer('L', size, s, "raw", 'L', 0, -1) 00051 remap = {0:255, 100: 0, 255:128} 00052 im = im.point(lambda x: remap.get(x, 0)) 00053 resolution = resp.map.info.resolution 00054 _map_cache[map_key] = (im, resolution) 00055 else: 00056 (im, resolution) = _map_cache[map_key] 00057 return (im, resolution) 00058 00059 def get_scaled_map(service_name, scale): 00060 scale_key = '%s:%s' % (service_name, scale) 00061 if not _scale_cache.has_key(scale_key): 00062 (im, resolution) = get_map(service_name) 00063 if scale != 1: 00064 size = im.size 00065 im = im.resize((int(size[0] / scale), int(size[1] / scale)), Image.ANTIALIAS) 00066 _scale_cache[scale_key] = im 00067 else: 00068 im = _scale_cache[scale_key] 00069 return im 00070 00071 def get_tile(service_name, scale, x, y, width, height): 00072 tile_key = '%s:%s:%s:%s:%s:%s' % (service_name, scale, x, y, width, height) 00073 if not _tile_cache.has_key(tile_key): 00074 # Get the map (possibly cached) from the map server 00075 im = get_scaled_map(service_name, scale) 00076 00077 # Crop the requested tile and convert to JPEG 00078 im = im.crop((x, y, x + width, y + height)) 00079 buf = cStringIO.StringIO() 00080 im.save(buf, format='JPEG') 00081 jpeg = buf.getvalue() 00082 _tile_cache[tile_key] = jpeg 00083 else: 00084 jpeg = _tile_cache[tile_key] 00085 return jpeg 00086 00087 def send_image(self, jpeg, cache_control='max-age=3600'): 00088 try: 00089 self.send_response(200) 00090 self.send_header('Cache-Control', cache_control) 00091 self.send_header('Content-Type', 'image/jpeg') 00092 self.send_header('Content-Length', str(len(jpeg))) 00093 self.end_headers() 00094 self.wfile.write(jpeg) 00095 except: 00096 pass 00097 00098 def get_tile_handler(self, path, qdict): 00099 service_name = qdict.get('service', ['static_map'])[0] 00100 scale = float(qdict.get('scale', [1])[0]) 00101 x = int(qdict.get('x', [0])[0]) 00102 y = int(qdict.get('y', [0])[0]) 00103 width = int(qdict.get('width', [256])[0]) 00104 height = int(qdict.get('height', [256])[0]) 00105 00106 try: 00107 jpeg = get_tile(service_name, scale, x, y, width, height) 00108 except Exception, reason: 00109 import base64 00110 jpeg = base64.decodestring(unavail.unavail) 00111 send_image(self, jpeg) 00112 00113 def get_pose(self, qdict): 00114 x = float(qdict.get('x', [0])[0]) 00115 y = float(qdict.get('y', [0])[0]) 00116 angle = float(qdict.get('angle', [0])[0]) 00117 p = Point(x, y, 0) 00118 q = tf.transformations.quaternion_from_euler(0, 0, angle) 00119 q = Quaternion(q[0], q[1], q[2], q[3]) 00120 return Pose(p, q) 00121 00122 def set_goal_handler(self, path, qdict): 00123 pose = get_pose(self, qdict) 00124 h = rospy.Header() 00125 h.stamp= rospy.get_rostime() 00126 h.frame_id = '/map' 00127 goal = PoseStamped(h, pose) 00128 goal_publisher.publish(goal) 00129 self.send_success() 00130 00131 def set_pose_handler(self, path, qdict): 00132 pose = get_pose(self, qdict) 00133 h = rospy.Header() 00134 h.stamp= rospy.get_rostime() 00135 h.frame_id = '/map' 00136 cov = [float64(0)] * 36 00137 cov[6*0+0] = 0.5 * 0.5; 00138 cov[6*1+1] = 0.5 * 0.5; 00139 cov[6*3+3] = math.pi/12.0 * math.pi/12.0; 00140 pose_publisher.publish(PoseWithCovarianceStamped(h, PoseWithCovariance(pose, cov))) 00141 self.send_success() 00142 00143 def get_extents_handler(self, path, qdict): 00144 service_name = qdict.get('service', ['static_map'])[0] 00145 callback = qdict.get("callback", [''])[0] 00146 (im, resolution) = get_map(service_name) 00147 buf = cStringIO.StringIO() 00148 buf.write('{') 00149 buf.write('"width": %d,' % im.size[0]) 00150 buf.write('"height": %d,' % im.size[1]) 00151 buf.write('"resolution": %s,' % resolution) 00152 buf.write('}') 00153 buf = buf.getvalue() 00154 self.send_success(buf, callback) 00155 00156 def map_tiler_handler(self, path, qdict): 00157 try: 00158 _lock.acquire() 00159 00160 if path == '/map/get_tile': get_tile_handler(self, path, qdict) 00161 elif path == '/map/get_extents': get_extents_handler(self, path, qdict) 00162 elif path == '/map/set_goal': set_goal_handler(self, path, qdict) 00163 elif path == '/map/set_pose': set_pose_handler(self, path, qdict) 00164 else: 00165 return False 00166 return True 00167 finally: 00168 _lock.release()