parse_world.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from xml.dom import minidom
00004 import os
00005 import urllib
00006 import urlparse
00007 import sys
00008 
00009 reload(sys)
00010 sys.setdefaultencoding('utf-8')
00011 
00012 
00013 def search_gazebo_dir (dirname, gpath = ['.']):
00014     for pdir in gpath:
00015         if os.access(pdir+'/'+dirname, os.F_OK):
00016             return pdir+'/'+dirname
00017 
00018 def resolve_gazebo_path (fname):
00019     if fname.find('model://') >= 0:
00020         up = urlparse.urlparse(fname)
00021         dname = up.netloc
00022         try:
00023             dirs = os.environ['GAZEBO_MODEL_PATH'].split(':')
00024             dirs.append('/tmp')
00025         except:
00026             print ""
00027             return fname
00028         gp = search_gazebo_dir (dname, gpath = dirs)
00029         if gp:
00030             return gp
00031         else:
00032             print ""
00033             return fname
00034     elif fname.find('file://media') >= 0:
00035         up = urlparse.urlparse(fname)
00036         dname = up.netloc + up.path
00037         try:
00038             dirs = os.environ['GAZEBO_RESOURCE_PATH'].split(':')
00039         except:
00040             print ""
00041             return fname
00042         gp = search_gazebo_dir (dname, gpath = dirs)
00043         if gp:
00044             return gp
00045         else:
00046             print ""
00047             return fname
00048     elif fname.find('package://') >= 0:
00049         return
00050     return fname
00051 
00052 def check_model_url (model):
00053     model = resolve_gazebo_path(model)
00054     pos = model.find('model://')
00055     if pos == -1:
00056         return model
00057     modelname = model[pos+8:] ## remove model://
00058     sys.stderr.write(";; model = %s\n"%modelname)
00059     tmpmodel = '/tmp/%s' % modelname
00060     if os.access(tmpmodel, os.F_OK):
00061         sys.stderr.write(";; at TMP: %s\n"%modelname)
00062         return tmpmodel
00063     fp = urllib.urlopen("http://gazebosim.org/models/%s/model.tar.gz" % modelname)
00064     if fp.getcode() == 200:
00065         f = open("/tmp/model.tar.gz", 'wb')
00066         f.write(fp.read())
00067         fp.close()
00068         f.close()
00069         os.system('tar xf /tmp/model.tar.gz -C /tmp')
00070         if os.access(tmpmodel, os.F_OK):
00071             sys.stderr.write(";; create at TMP: %s\n"%modelname)
00072             return tmpmodel
00073     return model
00074 
00075 def getText(nodelist):
00076     rc = []
00077     for node in nodelist:
00078         if node.nodeType == node.TEXT_NODE:
00079             rc.append(node.data)
00080     return ''.join(rc)
00081 
00082 if __name__ == '__main__':
00083     argvs = sys.argv
00084     argc = len(argvs)
00085 
00086     global doc, worldf
00087     if argc > 1:
00088         worldf = argvs[1]
00089     else:
00090         exit(0)
00091 
00092     doc = minidom.parse(worldf)
00093 
00094     wrd = doc.getElementsByTagName('world')
00095 
00096     if wrd != []:
00097         wrd_nm = wrd[0].getAttribute('name')
00098         print "(list"
00099         for inc in wrd[0].getElementsByTagName('include'):
00100             pose = ''
00101             name = ''
00102             modelf = ''
00103 
00104             elem = inc.getElementsByTagName('uri')
00105             if elem:
00106                 modelf = getText(elem[0].childNodes)
00107                 modelf =  check_model_url(modelf)
00108 
00109             elem = inc.getElementsByTagName('pose')
00110             if elem:
00111                 pose = getText(elem[0].childNodes)
00112 
00113             elem = inc.getElementsByTagName('name')
00114             if elem:
00115                 name = getText(elem[0].childNodes)
00116 
00117             print "(list (list :model \"%s\")" % modelf
00118             if pose != '':
00119                 print "      (list :pose \"%s\")" % pose
00120             if name != '':
00121                 print "      (list :name \"%s\")" % name
00122             print ")"
00123 
00124         for mdl in wrd[0].getElementsByTagName('model'):
00125             nm = mdl.getAttribute('name')
00126             if nm:
00127                 nm = nm + '_%s'%wrd_nm
00128                 dirname = '/tmp/%s' % nm
00129                 if not os.access(dirname, os.F_OK):
00130                     os.makedirs(dirname)
00131                 f = open('%s/model.sdf' % dirname, 'wb')
00132                 f.write('<?xml version="1.0"?>\n<sdf version="1.3">\n')
00133                 f.write(mdl.toxml())
00134                 f.write('</sdf>')
00135                 f.close()
00136                 print '(list (list :model \"%s\")' % dirname
00137                 pose = getText(mdl.getElementsByTagName('pose')[0].childNodes)
00138                 if pose:
00139                     print '      (list :pose  \"%s\")' % pose
00140                 print ')'
00141         print ")"


hrpsys_gazebo_atlas
Author(s): Yohei Kakiuchi
autogenerated on Thu Jun 6 2019 20:57:50