convert.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('simmechanics_to_urdf')
00004 import rospy
00005 import sys
00006 import tf
00007 from tf.transformations import euler_from_quaternion, quaternion_from_matrix
00008 import xml.dom.minidom
00009 from xml.dom.minidom import Document
00010 import threading
00011 import math
00012 import numpy
00013 import yaml
00014 
00015 # Conversion Factors
00016 INCH2METER = 0.0254
00017 SLUG2KG = 14.5939029
00018 SLUGININ2KGMM = .009415402
00019 MM2M = .001
00020 
00021 # Special Reference Frame(s)
00022 WORLD = "WORLD"
00023 
00024 # Arbitrary List of colors to give pieces different looks
00025 COLORS =[("green", "0 1 0 1"), ("black", "0 0 0 1"), ("red", "1 0 0 1"), 
00026          ("blue", "0 0 1 1"), ("yellow", "1 1 0 1"), ("pink", "1 0 1 1"), 
00027          ("cyan", "0 1 1 1"), ("green", "0 1 0 1"), ("white", "1 1 1 1"), 
00028          ("dblue", "0 0 .8 1"), ("dgreen", ".1 .8 .1 1"), ("gray", ".5 .5 .5 1")]
00029 
00030 class Converter:
00031         def __init__(self):
00032                 # initialize member variables
00033                 self.links = {}
00034                 self.frames = {}
00035                 self.joints = {}
00036                 self.names = {}
00037                 self.colormap = {}
00038                 self.colorindex = 0
00039                 self.usedcolors = {}
00040 
00041                 # Start the Transform Manager
00042                 self.tfman = TransformManager()
00043                 self.tfman.start()
00044 
00045                 # Extra Transforms for Debugging
00046                 self.tfman.add([0,0,0], [0.70682518,0,0,0.70682518], "ROOT", WORLD) # rotate so Z axis is up
00047 
00048         def convert(self, filename, configfile, mode):
00049                 self.mode = mode
00050 
00051                 # Parse the configuration file
00052                 self.parseConfig(configfile)
00053 
00054                 # Parse the input file
00055                 self.parse(xml.dom.minidom.parse(filename))
00056                 self.buildTree(self.root)
00057 
00058                 # Create the output 
00059                 self.result = Document()
00060                 self.output(self.root)
00061 
00062                 # output the output
00063                 if mode == "xml":
00064                         print self.result.toprettyxml()
00065                 if mode == "graph":
00066                         print self.graph()
00067                 if mode == "groups":
00068                         print self.groups(root)
00069 
00070         def parseConfig(self, configFile):
00071                 """Parse the Configuration File, if it exists.
00072                    Set the fields the default if the config does 
00073                    not set them """
00074                 if configFile == None:
00075                         configuration = {}
00076                 else:
00077                         configuration = yaml.load(file(configFile, 'r'))
00078                         if configuration == None:
00079                                 configuration = {}
00080 
00081                 self.freezeList = []
00082                 self.redefinedjoints = {}
00083 
00084                 self.root = configuration.get('root', None)
00085                 self.extrajoints = configuration.get('extrajoints', {})
00086                 self.filenameformat = configuration.get('filenameformat', "%s")
00087                 self.forcelowercase = configuration.get('forcelowercase', False)
00088                 self.scale = configuration.get('scale', None)
00089                 self.freezeAll = configuration.get('freezeAll', False)
00090                 self.baseframe = configuration.get('baseframe', WORLD)
00091 
00092                 # Get lists converted to strings
00093                 self.removeList = [ str(e) for e in configuration.get('remove', []) ]
00094                 self.freezeList = [ str(e) for e in configuration.get('freeze', []) ]
00095 
00096                 # Get map with key converted to strings
00097                 jointmap = configuration.get('redefinedjoints', {})
00098                 for x in jointmap.keys():
00099                         self.redefinedjoints[str(x)] = jointmap[x]
00100 
00101                 # Add Extra Frames
00102                 for frame in configuration.get('moreframes', []):
00103                         self.tfman.add(frame['offset'], frame['orientation'], frame['parent'], frame['child'])
00104                 
00105 
00106         def parse(self, element):
00107                 """Recursively goes through all XML elements 
00108                    and branches off for important elements"""
00109                 name = element.localName
00110                 # Grab name from root element AND recursively parse
00111                 if name == "PhysicalModelingXMLFile":
00112                         dict = getDictionary(element)
00113                         self.name = dict['name']
00114 
00115                 if name == "Body":
00116                         self.parseLink(element)
00117                 elif name == "SimpleJoint":
00118                         self.parseJoint(element)
00119                 elif name == "Ground":
00120                         dict = getDictionary(element)
00121                         self.parseFrames(dict['frame'], "GROUND")
00122                 else: 
00123                         for child in element.childNodes:
00124                                 self.parse(child)
00125 
00126         def parseLink(self, link):
00127                 """Parse the important bits of a link element"""
00128                 linkdict = getDictionary(link)
00129                 uid = self.getName(linkdict['name'])
00130                 linkdict['neighbors'] = []
00131                 linkdict['children'] = []
00132                 linkdict['jointmap'] = {}
00133 
00134                 # Save the frames for separate parsing
00135                 frames = linkdict['frames']
00136                 linkdict['frames'] = None
00137 
00138                 # Save the color if it exists
00139                 if 'MaterialProp' in linkdict:
00140                         colorelement = linkdict['MaterialProp'][1]
00141                         color = colorelement.childNodes[0].data
00142                         linkdict['MaterialProp'] = None
00143                         linkdict['color'] = color.replace(",", " ") + " 1"
00144                         
00145 
00146                 self.links[uid] = linkdict
00147                 self.parseFrames(frames, uid)
00148 
00149                 # Save First Actual Element as Root, if not defined already
00150                 if self.root == None and "geometryFileName" in linkdict:
00151                         self.root = uid
00152 
00153         def parseFrames(self, frames, parent):
00154                 """Parse the frames from xml"""
00155                 for frame in frames:
00156                         if frame.nodeType is frame.TEXT_NODE:
00157                                 continue
00158                         fdict = getDictionary(frame)
00159                         fid = str(frame.getAttribute("ref"))
00160                         fdict['parent'] = parent
00161 
00162                         offset = getlist(fdict['position'])
00163                         units = fdict['positionUnits']
00164                         for i in range(0, len(offset)):
00165                                 offset[i] = convert(offset[i], units)
00166 
00167                         orientation = getlist(fdict['orientation'])
00168                         quat = matrixToQuaternion(orientation)
00169                         
00170                         # If the frame does not have a reference number,
00171                         # use the name plus a suffix (for CG or CS1...
00172                         # otherwise ignore the frame
00173                         if fid == "":
00174                                 name = fdict['name']
00175                                 if name == "CG":
00176                                         fid = parent + "CG"
00177                                 elif name == "CS1":
00178                                         fid = parent + "CS1"
00179                                 else:
00180                                         continue
00181 
00182                         self.tfman.add(offset, quat, WORLD, fid)
00183                         self.frames[fid] = fdict
00184 
00185         def parseJoint(self, element):
00186                 """Parse the joint from xml"""
00187                 dict = getDictionary(element)
00188                 joint = {}
00189                 joint['name'] = dict['name']
00190                 uid = self.getName(joint['name'])
00191 
00192                 frames = element.getElementsByTagName("Frame")
00193                 joint['parent'] = str(frames[0].getAttribute("ref"))
00194                 joint['child'] = str(frames[1].getAttribute("ref"))
00195                 type = element.getElementsByTagName("Primitive")
00196 
00197                 # If there multiple elements, assume a fixed joint
00198                 if len(type)==1:
00199                         pdict = getDictionary(type[0])
00200                         joint['type'] = pdict['name']
00201                         joint['axis'] = pdict['axis']
00202                         if joint['type'] == 'weld':
00203                                 joint['type'] = 'fixed'
00204                 else:
00205                         joint['type'] = 'fixed'
00206 
00207                 # Ignore joints on the remove list
00208                 if joint['parent'] in self.removeList:
00209                         return
00210 
00211                 # Force joints to be fixed on the freezeList
00212                 if joint['parent'] in self.freezeList or self.freezeAll:
00213                         joint['type'] = 'fixed'
00214 
00215                 # Redefine specified joints on redefined list
00216                 if joint['parent'] in self.redefinedjoints.keys():
00217                         jdict = self.redefinedjoints[joint['parent']]
00218                         if 'name' in jdict:
00219                                 uid = jdict['name']
00220 
00221                         # Default to continuous joints
00222                         joint['type'] = jdict.get('type', 'continuous')
00223 
00224                         if 'axis' in jdict:
00225                                 joint['axis'] = jdict['axis']
00226                         if 'limits' in jdict:
00227                                 joint['limits'] = jdict['limits']
00228 
00229                 self.joints[uid] = joint
00230 
00231         def buildTree(self, root):
00232                 """Reduce the graph structure of links and joints to a tree
00233                    by breadth first search. Then construct new coordinate frames
00234                    from new tree structure"""
00235         
00236                 # Create a list of all neighboring links at each link
00237                 for jid in self.joints:
00238                         jointdict = self.joints[jid]
00239                         if "Root" in jointdict['name']:
00240                                 continue
00241                         pid = self.getLinkNameByFrame(jointdict['parent'])
00242                         cid = self.getLinkNameByFrame(jointdict['child'])
00243                         parent = self.links[pid]
00244                         child = self.links[cid]
00245 
00246                         parent['neighbors'].append(cid)
00247                         parent['jointmap'][cid] = jid
00248                         child['neighbors'].append(pid)
00249                         child['jointmap'][pid] = jid
00250 
00251                 # Add necessary information for any user-defined joints
00252                 for (name, extrajoint) in self.extrajoints.items():
00253                         pid = extrajoint['pid']
00254                         cid = extrajoint['cid']
00255                         jorigin = extrajoint['jorigin']
00256                         newframe = name + "_frame"
00257 
00258                         self.links[pid]['neighbors'].append(cid)
00259                         self.links[pid]['jointmap'][cid] = name
00260                         self.links[cid]['neighbors'].append(pid)
00261                         self.links[cid]['jointmap'][pid] = name
00262                         self.joints[name] = {'name': name, 'parent': jorigin, 'child': newframe}
00263                         for (k,v) in extrajoint['attributes'].items():
00264                                 self.joints[name][k] = v
00265                         self.frames[jorigin] = {'parent': pid}
00266                         self.frames[newframe] = {'parent': cid}
00267 
00268                 # Starting with designated root node, perform BFS to 
00269                 # create the tree
00270                 queue = [ root ]
00271                 self.links[root]['parent'] = "GROUND"
00272                 while len(queue) > 0:
00273                         id = queue.pop(0)
00274                         link = self.links[id]
00275                         for n in link['neighbors']:
00276                                 nbor = self.links[n]
00277                                 # if a neighbor has not been visited yet, 
00278                                 # add it as a child node
00279                                 if not 'parent' in nbor:
00280                                         nbor['parent'] = id
00281                                         queue.append(n)
00282                                         link['children'].append(n) 
00283 
00284                 # build new coordinate frames
00285                 for id in self.links:
00286                         link = self.links[id]
00287                         if not 'parent' in link:
00288                                 continue
00289                         parentid = link['parent']
00290                         if parentid == "GROUND":
00291                                 ref = self.baseframe
00292                         else:
00293                                 joint = self.joints[link['jointmap'][parentid]]
00294                                 ref = joint['parent']
00295                         # The root of each link is the offset to the joint
00296                         # and the rotation of the CS1 frame
00297                         (off1, rot1) = self.tfman.get(WORLD, ref)
00298                         (off2, rot2) = self.tfman.get(WORLD, id + "CS1")
00299                         self.tfman.add(off1, rot2, WORLD, "X" + id)
00300                         
00301 
00302         def output(self, rootid):
00303                 """Creates the URDF from the parsed document.
00304                    Makes the document and starts the recursive build process"""
00305                 doc = self.result;
00306                 self.root = doc.createElement("robot")
00307                 doc.appendChild(self.root)
00308                 self.root.setAttribute("name", self.name)
00309                 self.outputLink(rootid)
00310                 self.processLink(rootid)
00311 
00312         def processLink(self, id):
00313                 """ Creates the output for the specified node's
00314                     child links, the connecting joints, then 
00315                     recursively processes each child """
00316                 link = self.links[id]
00317                 for cid in link['children']:
00318                         jid = link['jointmap'][cid]
00319                                                 
00320                         self.outputLink(cid)
00321                         self.outputJoint(jid, id)
00322                         self.processLink(cid)
00323 
00324         def outputLink(self, id):
00325                 """ Creates the URDF output for a single link """
00326                 doc = self.result
00327                 linkdict = self.links[id]                
00328                 if linkdict['name'] == "RootPart":
00329                         return
00330                 
00331                 link = doc.createElement("link")
00332                 link.setAttribute("name", id)
00333 
00334                 visual = doc.createElement("visual")
00335                 inertial = doc.createElement("inertial")
00336                 collision = doc.createElement("collision")
00337                 link.appendChild(visual)
00338                 link.appendChild(inertial)
00339                 link.appendChild(collision)
00340 
00341                 # Define Geometry
00342                 geometry = doc.createElement("geometry")
00343                 mesh = doc.createElement("mesh")
00344                 filename = linkdict['geometryFileName']
00345                 if self.forcelowercase:
00346                         filename = filename.lower()
00347                 
00348                 mesh.setAttribute("filename", self.filenameformat % filename)
00349                 if self.scale != None:
00350                         mesh.setAttribute("scale", self.scale)
00351                 geometry.appendChild(mesh)
00352                 visual.appendChild(geometry)
00353                 collision.appendChild(geometry.cloneNode(1))
00354 
00355                 # Define Inertial Frame
00356                 mass = doc.createElement("mass")
00357                 units = linkdict['massUnits']
00358                 massval = convert(float(linkdict['mass']), units)
00359                 mass.setAttribute("value", str(massval))
00360                 inertial.appendChild(mass)
00361 
00362                 inertia = doc.createElement("inertia")
00363                 matrix = getlist(linkdict["inertia"])
00364                 units = linkdict['inertiaUnits']
00365         
00366                 for i in range(0,len(matrix)):  
00367                         matrix[i] = convert(matrix[i], units)
00368         
00369                 inertia.setAttribute("ixx", str(matrix[0]))
00370                 inertia.setAttribute("ixy", str(matrix[1]))
00371                 inertia.setAttribute("ixz", str(matrix[2]))
00372                 inertia.setAttribute("iyy", str(matrix[4]))
00373                 inertia.setAttribute("iyz", str(matrix[5]))
00374                 inertia.setAttribute("izz", str(matrix[8]))
00375                 inertial.appendChild(inertia)
00376 
00377                 # Inertial origin is the center of gravity
00378                 iorigin = doc.createElement("origin")
00379                 (off, rot) = self.tfman.get("X" + id, id+"CG")
00380                 rpy = list(euler_from_quaternion(rot))
00381                 iorigin.setAttribute("xyz", " ".join(map(str,zero(off))))
00382                 iorigin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
00383                 inertial.appendChild(iorigin)
00384 
00385                 # Create link's origin
00386                 origin = doc.createElement("origin")
00387                 
00388                 # Visual offset is difference between origin and CS1
00389                 (off, rot) = self.tfman.get("X" + id, id+"CS1")
00390                 rpy = list(euler_from_quaternion(rot))
00391                 origin.setAttribute("xyz", " ".join(map(str,zero(off))))
00392                 origin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
00393                 visual.appendChild(origin)
00394                 collision.appendChild(origin.cloneNode(1))
00395 
00396                 # Define Material
00397                 material = doc.createElement("material")
00398                 
00399                 # Use specified color, if exists. Otherwise, get random color
00400                 if 'color' in linkdict:
00401                         cname = "%s_color"%id
00402                         rgba = linkdict['color']
00403                 else:
00404                         (cname, rgba) = self.getColor(linkdict['name'])
00405                 material.setAttribute("name", cname)
00406                 visual.appendChild(material)
00407                 
00408                 # If color has already been output, only output name
00409                 if not cname in self.usedcolors:
00410                         color = doc.createElement("color")
00411                         color.setAttribute("rgba", rgba)
00412                         material.appendChild(color)
00413                         self.usedcolors[cname] = True
00414                                 
00415                 self.root.appendChild(link)
00416 
00417         def getColor(self, s):
00418                 """ Gets a two element list containing a color name,
00419                     and it's rgba. The color selected is based on the mesh name.
00420                     If already seen, returns the saved color
00421                     Otherwise, returns the next available color"""
00422                 if s in self.colormap:
00423                         return self.colormap[s]
00424                 color = COLORS[self.colorindex]
00425                 self.colormap[s] = color
00426                 self.colorindex = (self.colorindex + 1) % len(COLORS)
00427                 return color
00428                 
00429         def outputJoint(self, id, parentname):
00430                 """ Outputs URDF for a single joint """
00431                 doc = self.result
00432                 jointdict = self.joints[id]
00433 
00434                 if "Root" in jointdict['name']:
00435                         return
00436 
00437                 joint = doc.createElement("joint")              
00438                 joint.setAttribute('name', id)
00439 
00440                 # Define the parent and child
00441                 pid = self.getLinkNameByFrame(jointdict['parent'])
00442                 cid = self.getLinkNameByFrame(jointdict['child'])
00443 
00444                 # If the original joint was reversed while building the tree,
00445                 # swap the two ids
00446                 if parentname != pid:
00447                         cid = pid
00448                         pid = parentname
00449                 
00450                 parent = doc.createElement("parent")
00451                 child = doc.createElement("child")
00452                 parent.setAttribute('link', pid)
00453                 child.setAttribute('link', cid)
00454                 joint.appendChild(parent)
00455                 joint.appendChild(child)
00456 
00457                 # Define joint type
00458                 jtype = jointdict['type']
00459                 joint.setAttribute('type', jtype)
00460                 if 'limits' in jointdict:
00461                         limit = doc.createElement("limit")
00462                         for (k,v) in jointdict['limits'].items():
00463                                 limit.setAttribute(str(k), str(v))
00464                         joint.appendChild(limit)
00465                 if 'axis' in jointdict and jtype != 'fixed':
00466                         axis = doc.createElement("axis")
00467                         xyz = jointdict['axis'].replace(',', ' ')
00468                         axis.setAttribute('xyz', xyz)
00469                         joint.appendChild(axis)
00470         
00471                 # Define the origin
00472                 origin = doc.createElement("origin")
00473                 (off, rot) = self.tfman.get("X" + pid, "X" + cid)
00474                 rpy = list(euler_from_quaternion(rot))
00475                 off = zero(off)
00476                 rpy = zero(rpy)
00477                 origin.setAttribute("xyz", " ".join(map(str,off)))
00478                 origin.setAttribute("rpy", " ".join(map(str,rpy)))
00479                 joint.appendChild(origin)
00480 
00481                 self.root.appendChild(joint)
00482 
00483         def getName(self, basename):
00484                 """Return a unique name of the format 
00485                    basenameD where D is the lowest number
00486                    to make the name unique"""
00487                 index = 1
00488                 name = basename + str(index)
00489                 while name in self.names:
00490                         index = index + 1
00491                         name = basename + str(index)
00492                 self.names[name] = 1
00493                 return name
00494 
00495         def getLinkNameByFrame(self, key):
00496                 """Gets the link name from the frame object"""
00497                 return self.frames[key]['parent']
00498 
00499         def graph(self):
00500                 """For debugging purposes, output a graphviz 
00501                    representation of the tree structure, noting
00502                    which joints have been reversed and which have 
00503                    been removed"""
00504                 graph = "digraph proe {\n"
00505                 for jkey in self.joints:
00506                         joint = self.joints[jkey]
00507                         pref = joint['parent']
00508                         cref = joint['child']
00509                         label = pref + ":" + cref
00510                         pkey = self.getLinkNameByFrame(pref)
00511                         ckey = self.getLinkNameByFrame(cref)
00512                         case = 'std'
00513                         if pkey != "GROUND":
00514                                 parent = self.links[pkey]
00515                                 if not ckey in parent['children']:
00516                                         child = self.links[ckey]
00517                                         if pkey in child['children']:
00518                                                 case = 'rev'
00519                                         else:
00520                                                 case = 'not'
00521                         pkey = pkey.replace("-", "_")
00522                         ckey = ckey.replace("-", "_")
00523 
00524                         if (case == 'std' or case == 'rev') and (joint['type'] != "fixed"):
00525                                 style = " penwidth=\"5\""
00526                         else:
00527                                 style = "";
00528 
00529                         if case == 'std':
00530                                 s = pkey + " -> " + ckey + " [ label = \""+label+"\""; 
00531                         elif case == 'not':
00532                                 s = pkey + " -> " + ckey + " [ label = \""+label+"\" color=\"yellow\""
00533                         elif case == 'rev':
00534                                 s = ckey + " -> " + pkey + " [ label = \""+label+"\" color=\"blue\""
00535                         s = s + style + "];"
00536                         
00537                         if not "Root" in s and "-> SCR_" not in s:
00538                                 graph = graph + s + "\n"
00539                 return graph + "}\n"
00540 
00541         def groups(self, root):
00542                 """ For planning purposes, print out lists of 
00543                     all the links between the different joints"""
00544                 self.groups = {}
00545                 self.makeGroup(root, "BASE")
00546                 s = ""
00547                 for key in self.groups.keys():
00548                         s = s + key + ":\n\t"
00549                         ids = self.groups[key]
00550                         for id in ids:
00551                                 s = s+id + " "
00552                         s = s + "\n\n"
00553                 return s
00554 
00555         def makeGroup(self, id, gid):
00556                 """ Helper function for recursively gathering
00557                     groups of joints. """
00558                 if gid in self.groups:
00559                         idlist = self.groups[gid]
00560                         idlist.append(id)
00561                 else:
00562                         idlist = [id]
00563                 self.groups[gid] = idlist
00564                 link = self.links[id]
00565                 for child in link['children']:
00566                         jid = link['jointmap'][child]
00567                         joint = self.joints[jid]
00568                         if joint['type'] == 'weld':
00569                                 ngid = gid
00570                         else:
00571                                 ngid = jid
00572                         
00573                         self.makeGroup(child, ngid)
00574 
00575 def getDictionary(tag):
00576         """Builds a dictionary from the specified xml tag
00577            where each child of the tag is entered into the dictionary
00578            with the name of the child tag as the key, and the contents
00579            as the value. Also removes quotes from quoted values"""
00580         x = {}
00581         for child in tag.childNodes:
00582                 if child.nodeType is child.TEXT_NODE:
00583                         continue
00584                 key = str(child.localName)
00585                 if len(child.childNodes) == 1:
00586                         data = str(child.childNodes[0].data)
00587                         if data[0] == '"' and data[-1] == '"':
00588                                 if len(data) != 2:
00589                                         x[key] = data[1:-1]
00590                         else:
00591                                 x[key] = data
00592                 else:
00593                         data = child.childNodes
00594                         x[key] = data
00595         return x
00596 
00597 def getlist(string):
00598         """Splits a string of comma delimited floats to
00599            a list of floats"""
00600         slist = string.split(",")
00601         flist = []
00602         for s in slist:
00603                 flist.append(float(s))
00604         return flist
00605 
00606 
00607 def convert(value, units):
00608         """Convert value from the specified units to mks units"""
00609         if units == 'kg' or units == 'm' or units == 'kg*m^2':
00610                 return value
00611         elif units == 'slug*in^2':
00612                 return value * SLUGININ2KGMM
00613         elif units == 'slug':
00614                 return value * SLUG2KG
00615         elif units == 'in':
00616                 return value * INCH2METER
00617         elif units == 'mm':
00618                 return value * MM2M
00619         else:
00620                 raise Exception("unsupported mass unit: %s" % units)
00621 
00622 def matrixToQuaternion(matrix):
00623         """Concert 3x3 rotation matrix into a quaternion"""
00624         (R11, R12, R13, R21, R22, R23, R31, R32, R33) = matrix
00625         # Build 4x4 matrix
00626         M = [[R11, R21, R31, 0],
00627              [R12, R22, R32, 0],
00628              [R13, R23, R33, 0],
00629              [0,   0,   0,   1]]
00630         A = numpy.array(M)
00631         [w,x,y,z] = quaternion_from_matrix(A) 
00632         return [w,x,y,z]
00633 
00634 def quaternion_to_rpy(quat):
00635         """Convert quaternion into roll pitch yaw list (in degrees)"""
00636         rpy = list(euler_from_quaternion(quat))
00637         for i in range(0, len(rpy)):
00638                 rpy[i] = rpy[i]*180/math.pi
00639         return rpy
00640 
00641 def zero(arr):
00642         """Converts any numbers less than 1e-7 to 0 in the array"""
00643         for i in range(0,len(arr)):
00644                 if math.fabs(arr[i]) < 1e-7:
00645                         arr[i] = 0
00646         return arr
00647 
00648 
00649 class TransformManager(threading.Thread):
00650         """Bridge to the ROS tf package. Constantly broadcasts
00651            all tfs, and retrieves them on demand."""
00652 
00653         def __init__(self):
00654                 threading.Thread.__init__(self)
00655                 self.running = True
00656                 self.tfs = []
00657                 self.listener = tf.TransformListener()
00658 
00659         def run(self):
00660                 """Broadcasts tfs until shutdown"""
00661                 rate = rospy.Rate(10.0)
00662                 br = tf.TransformBroadcaster()
00663                 while self.running:
00664                         for transform in self.tfs:
00665                                 br.sendTransform(transform['trans'], 
00666                                         transform['rot'],
00667                                         rospy.Time.now(),
00668                                         transform['child'],
00669                                         transform['parent'])
00670                         rate.sleep()
00671 
00672         def add(self, trans, rot, parent, child):
00673                 """Adds a new transform to the set"""
00674                 tf = {'trans':trans, 'rot':rot, 'child':child, 'parent':parent}
00675                 self.tfs.append(tf)
00676 
00677         def get(self, parent, child):
00678                 """Attempts to retrieve a transform"""
00679                 attempts = 0
00680                 rate = rospy.Rate(10.0)
00681                 while attempts < 50:
00682                         try:
00683                                 (off,rpy) = self.listener.lookupTransform(parent, child, rospy.Time(0))
00684                                 return [list(off), list(rpy)]
00685                         except (tf.LookupException, tf.ConnectivityException):
00686                                 attempts+=1
00687                                 rate.sleep()
00688                                 continue
00689 
00690                 raise Exception("Attempt to transform %s exceeded attempt limit" % (parent + "/" + child))
00691 
00692         def printTransform(self, parent, child):
00693                 """Attempts to print the specified transform"""
00694                 (off, rot) = self.get(parent, child)
00695                 rpy = quaternion_to_rpy(rot)
00696 
00697                 s = parent + "-->" + child
00698                 l = (30 - len(s))*" "
00699                 print "%s%s[%+.5f %+.5f %+.5f] \t [%+3.3f %+.3f %+.3f] \t [%+.6f %+.6f %+.6f %+.6f] " \
00700                         % (s, l, off[0], off[1], off[2], rpy[0], rpy[1], rpy[2], rot[0], rot[1], rot[2], rot[3])
00701 
00702         def kill(self):
00703                 """Stops thread from running"""
00704                 self.running = False
00705 
00706 if __name__ == '__main__':
00707         argc = len(sys.argv)
00708         if argc == 3:
00709                 filename = sys.argv[1]
00710                 config = None
00711                 mode = sys.argv[2]
00712         elif argc == 4:
00713                 filename = sys.argv[1]
00714                 config = sys.argv[2]
00715                 mode = sys.argv[3]
00716         else:
00717                 print "Usage: " + sys.argv[0] + " {XML filename} [configfile] {tf|xml|graph|groups|none}"
00718                 sys.exit(-1)
00719         try:
00720                 rospy.init_node('convert') 
00721                 con = Converter()
00722                 try:
00723                         con.convert(filename, config, mode)
00724                         while mode == "tf" and not rospy.is_shutdown():
00725                                 None
00726                 except Exception:
00727                         raise
00728                 finally:
00729                         con.tfman.kill()
00730                         
00731         except rospy.ROSInterruptException: pass
00732 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


simmechanics_to_urdf
Author(s): David Lu!!
autogenerated on Mon Aug 19 2013 11:02:04