Updates to simmechanics_to_urdf and resource_retriever

resource_retriever/src/resource_retriever/__init__.py

@@ -0,0 +1,36 @@
+import roslib; roslib.load_manifest('resource_retriever')
+import subprocess
+import urlgrabber, string
+
+PACKAGE_PREFIX = 'package://'
+
+def rospack_find(package):
+    process = subprocess.Popen(['rospack', 'find', package], shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
+    (stdout, stderr) = process.communicate()
+    if len(stderr) > 0:
+        raise Exception(stderr)
+    else:
+        return string.strip(stdout)
+
+def get_filename(url, use_protocol=True ):
+    mod_url = url
+    if url.find(PACKAGE_PREFIX) == 0:
+        mod_url = url[len(PACKAGE_PREFIX):]
+        pos = mod_url.find('/')
+        if pos == -1:
+            raise Exception("Could not parse package:// format into file:// format for "+url)
+
+        package = mod_url[0:pos]
+        mod_url = mod_url[pos:]
+        package_path = rospack_find(package)
+
+        if use_protocol:
+            protocol = "file://"
+        else:
+            protocol = ""
+        mod_url = protocol + package_path + mod_url;
+    return mod_url
+
+def get(url):
+    return urlgrabber.urlopen(get_filename(url))
+

resource_retriever/src/resource_retriever/retriever.py

@@ -1,31 +0,0 @@
-import roslib; roslib.load_manifest('resource_retriever2')
-import rospy
-import subprocess
-import urlgrabber, string
-
-PACKAGE_PREFIX = 'package://'
-
-def rospack_find(package):
-    process = subprocess.Popen(['rospack', 'find', package], shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
-    (stdout, stderr) = process.communicate()
-    if len(stderr) > 0:
-        raise Exception(stderr)
-    else:
-        return string.strip(stdout)
-
-def get(url):
-    mod_url = url
-    if url.find(PACKAGE_PREFIX) == 0:
-        mod_url = url[len(PACKAGE_PREFIX):]
-        pos = mod_url.find('/')
-        if pos == -1:
-            raise Exception("Could not parse package:// format into file:// format for "+url)
-
-        package = mod_url[0:pos]
-        mod_url = mod_url[pos:]
-        package_path = rospack_find(package)
-
-        mod_url = "file://" + package_path + mod_url;
-
-    return urlgrabber.urlopen(mod_url)	
-

simmechanics_to_urdf/convert.py

@@ -5,12 +5,13 @@ import rospy
 import sys
 import tf
 from tf.transformations import euler_from_quaternion, quaternion_from_matrix
+from geometry_msgs.msg import TransformStamped
 import xml.dom.minidom
 from xml.dom.minidom import Document
-import threading
 import math
-import numpy
+import numpy	#for creating matrix to convert to quaternion
 import yaml
+from urdf_python.urdf import *
 
 # Conversion Factors
 INCH2METER = 0.0254
@@ -22,10 +23,10 @@ MM2M = .001
 WORLD = "WORLD"
 
 # Arbitrary List of colors to give pieces different looks
-COLORS =[("green", "0 1 0 1"), ("black", "0 0 0 1"), ("red", "1 0 0 1"), 
+COLORS =[("green", (0, 1, 0, 1)), ("black", (0, 0, 0, 1)), ("red", (1, 0, 0, 1)), 
-	 ("blue", "0 0 1 1"), ("yellow", "1 1 0 1"), ("pink", "1 0 1 1"), 
+	 ("blue", (0, 0, 1, 1)), ("yellow", (1, 1, 0, 1)), ("pink", (1, 0, 1, 1)), 
-	 ("cyan", "0 1 1 1"), ("green", "0 1 0 1"), ("white", "1 1 1 1"), 
+	 ("cyan", (0, 1, 1, 1)), ("green", (0, 1, 0, 1)), ("white", (1, 1, 1, 1)), 
-	 ("dblue", "0 0 .8 1"), ("dgreen", ".1 .8 .1 1"), ("gray", ".5 .5 .5 1")]
+	 ("dblue", (0, 0, .8, 1)), ("dgreen", (.1, .8, .1, 1)), ("gray", (.5, .5, .5, 1))]
 
 class Converter:
 	def __init__(self):
@@ -40,7 +41,6 @@ class Converter:
 
 		# Start the Transform Manager
 		self.tfman = TransformManager()
-		self.tfman.start()
 
 		# Extra Transforms for Debugging
 		self.tfman.add([0,0,0], [0.70682518,0,0,0.70682518], "ROOT", WORLD) # rotate so Z axis is up
@@ -56,12 +56,11 @@ class Converter:
 		self.buildTree(self.root)
 
 		# Create the output 
-		self.result = Document()
 		self.output(self.root)
 
 		# output the output
 		if mode == "xml":
-			print self.result.toprettyxml()
+			print self.result.to_xml()
 		if mode == "graph":
 			print self.graph()
 		if mode == "groups":
@@ -140,8 +139,7 @@ class Converter:
 			colorelement = linkdict['MaterialProp'][1]
 			color = colorelement.childNodes[0].data
 			linkdict['MaterialProp'] = None
-			linkdict['color'] = color.replace(",", " ") + " 1"
+			linkdict['color'] = map(float, color.split(",")) + [1.0]
-			
 
 		self.links[uid] = linkdict
 		self.parseFrames(frames, uid)
@@ -302,10 +300,7 @@ class Converter:
 	def output(self, rootid):
 		"""Creates the URDF from the parsed document.
 		   Makes the document and starts the recursive build process"""
-		doc = self.result;
+		self.result = URDF(self.name)
-		self.root = doc.createElement("robot")
-		doc.appendChild(self.root)
-		self.root.setAttribute("name", self.name)
 		self.outputLink(rootid)
 		self.processLink(rootid)
 
@@ -323,96 +318,72 @@ class Converter:
 
 	def outputLink(self, id):
 		""" Creates the URDF output for a single link """
-		doc = self.result
+
 		linkdict = self.links[id]		 
 		if linkdict['name'] == "RootPart":
 			return
 
-		link = doc.createElement("link")
+		visual = Visual()
-		link.setAttribute("name", id)
+		inertial = Inertial()
-
+		collision = Collision()
-		visual = doc.createElement("visual")
-		inertial = doc.createElement("inertial")
-		collision = doc.createElement("collision")
-		link.appendChild(visual)
-		link.appendChild(inertial)
-		link.appendChild(collision)
 
 		# Define Geometry
-		geometry = doc.createElement("geometry")
-		mesh = doc.createElement("mesh")
 		filename = linkdict['geometryFileName']
 		if self.forcelowercase:
 			filename = filename.lower()
+		filename = self.filenameformat % filename
 
-		mesh.setAttribute("filename", self.filenameformat % filename)
+		visual.geometry = Mesh(filename, self.scale)
-		if self.scale != None:
+		collision.geometry = visual.geometry
-			mesh.setAttribute("scale", self.scale)
-		geometry.appendChild(mesh)
-		visual.appendChild(geometry)
-		collision.appendChild(geometry.cloneNode(1))
 
 		# Define Inertial Frame
-		mass = doc.createElement("mass")
 		units = linkdict['massUnits']
 		massval = convert(float(linkdict['mass']), units)
-		mass.setAttribute("value", str(massval))
+		inertial.mass = massval
-		inertial.appendChild(mass)
 
-		inertia = doc.createElement("inertia")
 		matrix = getlist(linkdict["inertia"])
+
 		units = linkdict['inertiaUnits']
 	
 		for i in range(0,len(matrix)):	
 			matrix[i] = convert(matrix[i], units)
 	
-		inertia.setAttribute("ixx", str(matrix[0]))
+		inertial.matrix['ixx'] = matrix[0]
-		inertia.setAttribute("ixy", str(matrix[1]))
+		inertial.matrix['ixy'] = matrix[1]
-		inertia.setAttribute("ixz", str(matrix[2]))
+		inertial.matrix['ixz'] = matrix[2]
-		inertia.setAttribute("iyy", str(matrix[4]))
+		inertial.matrix['iyy'] = matrix[4]
-		inertia.setAttribute("iyz", str(matrix[5]))
+		inertial.matrix['iyz'] = matrix[5]
-		inertia.setAttribute("izz", str(matrix[8]))
+		inertial.matrix['izz'] = matrix[8]
-		inertial.appendChild(inertia)
 
 		# Inertial origin is the center of gravity
-		iorigin = doc.createElement("origin")
 		(off, rot) = self.tfman.get("X" + id, id+"CG")
 		rpy = list(euler_from_quaternion(rot))
-		iorigin.setAttribute("xyz", " ".join(map(str,zero(off))))
+		inertial.origin = Pose(zero(off), zero(rpy))
-		iorigin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
-		inertial.appendChild(iorigin)
-
-		# Create link's origin
-		origin = doc.createElement("origin")
 
 		# Visual offset is difference between origin and CS1
 		(off, rot) = self.tfman.get("X" + id, id+"CS1")
 		rpy = list(euler_from_quaternion(rot))
-		origin.setAttribute("xyz", " ".join(map(str,zero(off))))
+		visual.origin = Pose(zero(off), zero(rpy))
-		origin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
+		collision.origin = visual.origin
-		visual.appendChild(origin)
-		collision.appendChild(origin.cloneNode(1))
 		
 		# Define Material
-		material = doc.createElement("material")
+		visual.material = Material()
-		
 		# Use specified color, if exists. Otherwise, get random color
 		if 'color' in linkdict:
 			cname = "%s_color"%id
-			rgba = linkdict['color']
+			(r,g,b,a) = linkdict['color']
 		else:
-			(cname, rgba) = self.getColor(linkdict['name'])
+			(cname, (r,g,b,a)) = self.getColor(linkdict['name'])
-		material.setAttribute("name", cname)
+
-		visual.appendChild(material)
+		visual.material.name = cname
 		
 		# If color has already been output, only output name
 		if not cname in self.usedcolors:
-			color = doc.createElement("color")
+			visual.material.color = Color(r,g,b,a)
-			color.setAttribute("rgba", rgba)
-			material.appendChild(color)
 			self.usedcolors[cname] = True
 
-		self.root.appendChild(link)
+		link = Link(id, visual, inertial, collision)
+		self.result.add_link(link)
 
 	def getColor(self, s):
 		""" Gets a two element list containing a color name,
@@ -428,14 +399,11 @@ class Converter:
 		
 	def outputJoint(self, id, parentname):
 		""" Outputs URDF for a single joint """
-		doc = self.result
 		jointdict = self.joints[id]
 
 		if "Root" in jointdict['name']:
 			return
 
-		joint = doc.createElement("joint")		
-		joint.setAttribute('name', id)
 
 		# Define the parent and child
 		pid = self.getLinkNameByFrame(jointdict['parent'])
@@ -447,38 +415,27 @@ class Converter:
 			cid = pid
 			pid = parentname
 
-		parent = doc.createElement("parent")
-		child = doc.createElement("child")
-		parent.setAttribute('link', pid)
-		child.setAttribute('link', cid)
-		joint.appendChild(parent)
-		joint.appendChild(child)
-
 		# Define joint type
 		jtype = jointdict['type']
-		joint.setAttribute('type', jtype)
+
+		limits = None
+		axis = None
+
 		if 'limits' in jointdict:
-			limit = doc.createElement("limit")
+			limits = JointLimit(None, None)
 			for (k,v) in jointdict['limits'].items():
-				limit.setAttribute(str(k), str(v))
+				setattr(limits, k, v)
-			joint.appendChild(limit)
+
 		if 'axis' in jointdict and jtype != 'fixed':
-			axis = doc.createElement("axis")
+			axis = jointdict['axis'].replace(',', ' ')
-			xyz = jointdict['axis'].replace(',', ' ')
-			axis.setAttribute('xyz', xyz)
-			joint.appendChild(axis)
 	
 		# Define the origin
-		origin = doc.createElement("origin")
 		(off, rot) = self.tfman.get("X" + pid, "X" + cid)
 		rpy = list(euler_from_quaternion(rot))
-		off = zero(off)
+		origin = Pose(zero(off), zero(rpy))
-		rpy = zero(rpy)
-		origin.setAttribute("xyz", " ".join(map(str,off)))
-		origin.setAttribute("rpy", " ".join(map(str,rpy)))
-		joint.appendChild(origin)
 
-		self.root.appendChild(joint)
+		joint = Joint(id, pid, cid, jtype, limits=limits, axis=axis, origin=origin)
+		self.result.add_joint(joint)
 
 	def getName(self, basename):
 		"""Return a unique name of the format 
@@ -646,48 +603,28 @@ def zero(arr):
 	return arr
 
 
-class TransformManager(threading.Thread):
+class TransformManager:
-	"""Bridge to the ROS tf package. Constantly broadcasts
-	   all tfs, and retrieves them on demand."""
-
 	def __init__(self):
-		threading.Thread.__init__(self)
+		self.tf = tf.Transformer(True, rospy.Duration(10.0))
-		self.running = True
-		self.tfs = []
-		self.listener = tf.TransformListener()
 
-	def run(self):
+	def add(self, offset, angle, parent, child):
-		"""Broadcasts tfs until shutdown"""
-		rate = rospy.Rate(10.0)
-		br = tf.TransformBroadcaster()
-		while self.running:
-			for transform in self.tfs:
-				br.sendTransform(transform['trans'], 
-					transform['rot'],
-					rospy.Time.now(),
-					transform['child'],
-					transform['parent'])
-			rate.sleep()
-
-	def add(self, trans, rot, parent, child):
 		"""Adds a new transform to the set"""
-		tf = {'trans':trans, 'rot':rot, 'child':child, 'parent':parent}
+		m = TransformStamped()
-		self.tfs.append(tf)
+		m.header.frame_id = parent
+		m.child_frame_id = child
+		m.transform.translation.x = offset[0]
+		m.transform.translation.y = offset[1]
+		m.transform.translation.z = offset[2]
+		m.transform.rotation.x = angle[0]
+		m.transform.rotation.y = angle[1]
+		m.transform.rotation.z = angle[2]
+		m.transform.rotation.w = angle[3]
+		self.tf.setTransform(m)
 
 	def get(self, parent, child):
-		"""Attempts to retrieve a transform"""
+		"""Retrieves a transform"""
-		attempts = 0
+		(off,rpy) = self.tf.lookupTransform(parent, child, rospy.Time(0))
-		rate = rospy.Rate(10.0)
-		while attempts < 50:
-			try:
-				(off,rpy) = self.listener.lookupTransform(parent, child, rospy.Time(0))
 		return [list(off), list(rpy)]
-			except (tf.LookupException, tf.ConnectivityException):
-				attempts+=1
-				rate.sleep()
-				continue
-
-		raise Exception("Attempt to transform %s exceeded attempt limit" % (parent + "/" + child))
 
 	def printTransform(self, parent, child):
 		"""Attempts to print the specified transform"""
@@ -725,8 +662,6 @@ if __name__ == '__main__':
 				None
 		except Exception:
 			raise
-		finally:
-			con.tfman.kill()
 			
 	except rospy.ROSInterruptException: pass
 

simmechanics_to_urdf/manifest.xml

@@ -9,6 +9,7 @@
   <depend package="rospy"/>
   <depend package="sensor_msgs"/>
   <depend package="tf"/>
+  <depend package="urdf_python"/>
   <rosdep name="python-yaml"/>
 </package>