00001 from collections import OrderedDict 00002 00003 BASIC_FRAME = """<!DOCTYPE html> 00004 <html> 00005 <head> 00006 <meta charset="utf-8" /> 00007 00008 %(headers)s 00009 00010 <script type="text/javascript"> 00011 %(main_script)s 00012 </script> 00013 </head> 00014 00015 <body onload="init()"> 00016 <div id="%(div_id)s"></div> 00017 </body> 00018 </html> 00019 """ 00020 00021 INCLUDE_TEMPLATE = '<script type="text/javascript" src="http://cdn.robotwebtools.org/%s"></script>' 00022 00023 REQUIRED_HEADERS = ['threejs/current/three.min.js', 'EventEmitter2/current/eventemitter2.min.js', 00024 'roslibjs/current/roslib.min.js', 'ros3djs/current/ros3d.min.js'] 00025 00026 COLLADA_HEADERS = ['threejs/current/ColladaLoader.min.js', 'ColladaAnimationCompress/current/ColladaLoader2.min.js'] 00027 BSON_HEADER = 'js-bson/current/bson.min.js' 00028 00029 SCRIPT = """ 00030 /** 00031 * Setup all visualization elements when the page is loaded. 00032 */ 00033 function init() { 00034 // Connect to ROS. 00035 var ros = new ROSLIB.Ros({ 00036 url : 'ws://%(host)s:9090' 00037 }); 00038 00039 // Create the main viewer. 00040 var viewer = new ROS3D.Viewer({ 00041 divID : '%(div_id)s', 00042 width : %(width)d, 00043 height : %(height)d, 00044 antialias : true 00045 }); 00046 00047 %(objects)s 00048 } 00049 """ 00050 00051 OBJECT_TEMPLATE = """%(comment)s var %(name)s = new %(type)s(%(dict)s);\n\n""" 00052 00053 def quote(s): 00054 if s is None: 00055 return None 00056 elif len(s)>0 and (s[0]=='"' or s[0]=="'"): 00057 return s 00058 else: 00059 return "'%s'"%s 00060 00061 def double(d): 00062 if d is None: 00063 return None 00064 else: 00065 return d*2 00066 00067 class RWTConfig: 00068 def __init__(self, host=None, div_id='robotdisplay', size=(800,600), fixed_frame=None): 00069 if host is None: 00070 host = 'localhost' 00071 self.div_id = div_id 00072 self.fixed_frame = fixed_frame 00073 self.params = {'host': host, 'width': size[0], 'height': size[1], 'div_id': div_id} 00074 self.headers = REQUIRED_HEADERS 00075 self.names = set() 00076 self.tf = None 00077 self.objects = [] 00078 00079 def add_object(self, d): 00080 self.objects.append(d) 00081 00082 00083 def add_tf_client(self, name='tfClient', angularThres=0.01, transThres=0.01, rate=10.0, comment='Setup a client to listen to TFs'): 00084 if self.tf: 00085 return self.tf 00086 self.tf = name 00087 d = OrderedDict() 00088 d['name'] = name 00089 d['type'] = 'ROSLIB.TFClient' 00090 d['comment'] = 'Setup a client to listen to TFs.' 00091 d['ros'] = 'ros' 00092 d['angularThres'] = angularThres 00093 d['transThres'] = transThres 00094 d['rate'] = rate 00095 if self.fixed_frame: 00096 d['fixedFrame'] = quote(self.fixed_frame) 00097 00098 self.add_object(d) 00099 00100 return self.tf 00101 00102 def add_grid(self, comment='Add a grid.'): 00103 s = '' 00104 if comment: 00105 s += ' '*4 + '// ' + comment + '\n' 00106 s += ' '*4 + 'viewer.addObject(new ROS3D.Grid());' 00107 self.add_object(s) 00108 00109 def add_model(self, param=None, path='http://resources.robotwebtools.org/', tfPrefix=None, comment='Setup the URDF client.'): 00110 d = OrderedDict() 00111 00112 self.headers += COLLADA_HEADERS 00113 00114 d['type'] = 'ROS3D.UrdfClient' 00115 d['comment'] = comment 00116 d['ros'] = 'ros' 00117 d['param'] = quote(param) 00118 d['tfClient'] = self.add_tf_client() 00119 d['tfPrefix'] = quote(tfPrefix) 00120 d['path'] = quote(path) 00121 d['rootObject'] = 'viewer.scene' 00122 d['loader'] = 'ROS3D.COLLADA_LOADER_2' 00123 00124 self.add_object(d) 00125 00126 def add_map(self, name='gridClient', topic=None, tf=False, alpha=None, comment='Setup the map client.'): 00127 d = OrderedDict() 00128 d['name'] = name 00129 d['type'] = 'ROS3D.OccupancyGridClient' 00130 d['comment'] = comment 00131 d['ros'] = 'ros' 00132 d['topic'] = quote(topic) 00133 d['opacity'] = alpha 00134 if tf: 00135 d['tfClient'] = self.add_tf_client() 00136 d['rootObject'] = 'viewer.scene' 00137 00138 self.add_object(d) 00139 00140 def add_markers(self, name=None, topic='/visualization_marker', comment='Setup the marker client.'): 00141 d = OrderedDict() 00142 d['name'] = name 00143 d['type'] = 'ROS3D.MarkerClient' 00144 d['comment'] = comment 00145 d['ros'] = 'ros' 00146 d['tfClient'] = self.add_tf_client() 00147 d['topic'] = quote(topic) 00148 d['rootObject'] = 'viewer.scene' 00149 00150 self.add_object(d) 00151 00152 def add_marker_array(self, name=None, topic='/visualization_marker_array', comment='Setup the marker client.'): 00153 d = OrderedDict() 00154 d['name'] = name 00155 d['type'] = 'ROS3D.MarkerArrayClient' 00156 d['comment'] = comment 00157 d['ros'] = 'ros' 00158 d['tfClient'] = self.add_tf_client() 00159 d['topic'] = quote(topic) 00160 d['rootObject'] = 'viewer.scene' 00161 00162 self.add_object(d) 00163 00164 def add_imarkers(self, name='imClient', topic='/interactive_markers', comment='Setup the marker client.'): 00165 d = OrderedDict() 00166 d['name'] = name 00167 d['type'] = 'ROS3D.InteractiveMarkerClient' 00168 d['comment'] = comment 00169 d['ros'] = 'ros' 00170 d['tfClient'] = self.add_tf_client() 00171 d['topic'] = quote(topic) 00172 d['camera'] = 'viewer.camera' 00173 d['rootObject'] = 'viewer.selectableObjects' 00174 00175 self.add_object(d) 00176 00177 def add_pointcloud(self, name=None, topic='/points', size=None, max_pts=None, comment='Setup the Point Cloud client.'): 00178 d = OrderedDict() 00179 d['name'] = name 00180 d['type'] = 'ROS3D.PointCloud2' 00181 d['comment'] = comment 00182 d['ros'] = 'ros' 00183 d['tfClient'] = self.add_tf_client() 00184 d['topic'] = quote(topic) 00185 d['size'] = size 00186 d['max_pts'] = max_pts 00187 d['rootObject'] = 'viewer.scene' 00188 00189 self.add_object(d) 00190 00191 def add_laserscan(self, name=None, topic='/scan', color=None, size=None, max_pts=None, comment='Setup the LaserScan client.'): 00192 d = OrderedDict() 00193 d['name'] = name 00194 d['type'] = 'ROS3D.LaserScan' 00195 d['comment'] = comment 00196 d['ros'] = 'ros' 00197 d['tfClient'] = self.add_tf_client() 00198 d['topic'] = quote(topic) 00199 d['size'] = size 00200 d['max_pts'] = max_pts 00201 d['rootObject'] = 'viewer.scene' 00202 00203 self.add_object(d) 00204 00205 def add_path(self, name=None, topic='/path', color=None, comment='Setup the Path client.'): 00206 d = OrderedDict() 00207 d['name'] = name 00208 d['type'] = 'ROS3D.Path' 00209 d['comment'] = comment 00210 d['ros'] = 'ros' 00211 d['tfClient'] = self.add_tf_client() 00212 d['topic'] = quote(topic) 00213 d['color'] = color 00214 d['rootObject'] = 'viewer.scene' 00215 00216 self.add_object(d) 00217 00218 def add_polygon(self, name=None, topic='/polygon', color=None, comment='Setup the Path client.'): 00219 d = OrderedDict() 00220 d['name'] = name 00221 d['type'] = 'ROS3D.Polygon' 00222 d['comment'] = comment 00223 d['ros'] = 'ros' 00224 d['tfClient'] = self.add_tf_client() 00225 d['topic'] = quote(topic) 00226 d['color'] = color 00227 d['rootObject'] = 'viewer.scene' 00228 00229 self.add_object(d) 00230 00231 def add_pose(self, name=None, topic='/pose', color=None, shaft_radius=None, head_radius=None, 00232 shaft_length=None, head_length=None, comment=''): 00233 d = OrderedDict() 00234 d['name'] = name 00235 d['type'] = 'ROS3D.Pose' 00236 d['comment'] = comment 00237 d['ros'] = 'ros' 00238 d['tfClient'] = self.add_tf_client() 00239 d['topic'] = quote(topic) 00240 d['color'] = color 00241 d['length'] = shaft_length 00242 d['headLength'] = head_length 00243 d['shaftDiameter'] = double(shaft_radius) 00244 d['headDiameter'] = double(head_radius) 00245 d['rootObject'] = 'viewer.scene' 00246 00247 self.add_object(d) 00248 00249 def add_odometry(self, name=None, topic='/pose', color=None, keep=None, shaft_radius=None, head_radius=None, 00250 shaft_length=None, head_length=None, comment=''): 00251 d = OrderedDict() 00252 d['name'] = name 00253 d['type'] = 'ROS3D.Odometry' 00254 d['comment'] = comment 00255 d['ros'] = 'ros' 00256 d['tfClient'] = self.add_tf_client() 00257 d['topic'] = quote(topic) 00258 d['color'] = color 00259 d['keep'] = keep 00260 d['length'] = shaft_length 00261 d['headLength'] = head_length 00262 d['shaftDiameter'] = double(shaft_radius) 00263 d['headDiameter'] = double(head_radius) 00264 d['rootObject'] = 'viewer.scene' 00265 00266 self.add_object(d) 00267 00268 def add_posearray(self, name=None, topic='/particlecloud', color=None, length=None, comment='Setup the PoseArray client.'): 00269 d = OrderedDict() 00270 d['name'] = name 00271 d['type'] = 'ROS3D.PoseArray' 00272 d['comment'] = comment 00273 d['ros'] = 'ros' 00274 d['tfClient'] = self.add_tf_client() 00275 d['topic'] = quote(topic) 00276 d['color'] = color 00277 d['length'] = length 00278 d['rootObject'] = 'viewer.scene' 00279 00280 self.add_object(d) 00281 00282 def add_point(self, name=None, topic='/point', color=None, radius=None, comment='Setup the Point client.'): 00283 d = OrderedDict() 00284 d['name'] = name 00285 d['type'] = 'ROS3D.Point' 00286 d['comment'] = comment 00287 d['ros'] = 'ros' 00288 d['tfClient'] = self.add_tf_client() 00289 d['topic'] = quote(topic) 00290 d['color'] = color 00291 d['radius'] = radius 00292 d['rootObject'] = 'viewer.scene' 00293 00294 self.add_object(d) 00295 00296 def get_main_script(self): 00297 d = {} 00298 d.update(self.params) 00299 object_strings = [] 00300 for o in self.objects: 00301 if type(o)==str: 00302 object_strings.append(o) 00303 else: 00304 object_strings.append( self.get_object(o) ) 00305 d['objects'] = '\n'.join( object_strings ) 00306 return SCRIPT % d 00307 00308 def get_object(self, params): 00309 p2 = params.copy() 00310 d = {} 00311 d['type'] = p2['type'] 00312 del p2['type'] 00313 00314 if 'name' in p2 and p2['name'] is not None: 00315 d['name'] = p2['name'] 00316 del p2['name'] 00317 else: 00318 name = d['type'] 00319 if '.' in name: 00320 _, name = name.split('.', 1) 00321 name = name[0].lower() + name[1:] 00322 base = name 00323 c = 0 00324 while name in self.names: 00325 c+=1 00326 name = '%s_%d'%(base, c) 00327 self.names.add(name) 00328 00329 d['name'] = name 00330 00331 if 'comment' in p2: 00332 d['comment'] = ' // %s\n'%p2['comment'] 00333 del p2['comment'] 00334 else: 00335 d['comment'] = '' 00336 00337 if len(p2)>0: 00338 ps = [] 00339 for k,v in p2.iteritems(): 00340 if v is None: 00341 continue 00342 ps.append(' %s : %s'%(k, str(v))) 00343 d['dict'] = '{\n%s\n }'%',\n'.join(ps) 00344 else: 00345 d['dict'] = '{}' 00346 00347 return OBJECT_TEMPLATE % d 00348 00349 def get_headers(self): 00350 return '\n'.join([INCLUDE_TEMPLATE%s for s in self.headers]) 00351 00352 def add_bson_header(self): 00353 self.headers.append(BSON_HEADER) 00354 00355 def __repr__(self): 00356 d = {} 00357 d['div_id'] = self.div_id 00358 d['headers'] = self.get_headers() 00359 d['main_script'] = self.get_main_script() 00360 return BASIC_FRAME % d