diff --git a/resource_retriever/src/resource_retriever/__init__.py b/resource_retriever/src/resource_retriever/__init__.py
index e69de29..3f8d855 100644
--- a/resource_retriever/src/resource_retriever/__init__.py
+++ b/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))
+
diff --git a/resource_retriever/src/resource_retriever/retriever.py b/resource_retriever/src/resource_retriever/retriever.py
deleted file mode 100644
index f6edbac..0000000
--- a/resource_retriever/src/resource_retriever/retriever.py
+++ /dev/null
@@ -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)
-
diff --git a/simmechanics_to_urdf/convert.py b/simmechanics_to_urdf/convert.py
index 412590e..7d67c0d 100755
--- a/simmechanics_to_urdf/convert.py
+++ b/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"),
- ("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"),
- ("dblue", "0 0 .8 1"), ("dgreen", ".1 .8 .1 1"), ("gray", ".5 .5 .5 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)),
+ ("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))]
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,17 +300,14 @@ 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.root = doc.createElement("robot")
- doc.appendChild(self.root)
- self.root.setAttribute("name", self.name)
+ self.result = URDF(self.name)
self.outputLink(rootid)
self.processLink(rootid)
def processLink(self, id):
""" Creates the output for the specified node's
- child links, the connecting joints, then
- recursively processes each child """
+ child links, the connecting joints, then
+ recursively processes each child """
link = self.links[id]
for cid in link['children']:
jid = link['jointmap'][cid]
@@ -323,102 +318,78 @@ 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")
- link.setAttribute("name", id)
- visual = doc.createElement("visual")
- inertial = doc.createElement("inertial")
- collision = doc.createElement("collision")
- link.appendChild(visual)
- link.appendChild(inertial)
- link.appendChild(collision)
+ visual = Visual()
+ inertial = Inertial()
+ collision = Collision()
# Define Geometry
- geometry = doc.createElement("geometry")
- mesh = doc.createElement("mesh")
filename = linkdict['geometryFileName']
if self.forcelowercase:
filename = filename.lower()
-
- mesh.setAttribute("filename", self.filenameformat % filename)
- if self.scale != None:
- mesh.setAttribute("scale", self.scale)
- geometry.appendChild(mesh)
- visual.appendChild(geometry)
- collision.appendChild(geometry.cloneNode(1))
+ filename = self.filenameformat % filename
+
+ visual.geometry = Mesh(filename, self.scale)
+ collision.geometry = visual.geometry
# Define Inertial Frame
- mass = doc.createElement("mass")
units = linkdict['massUnits']
massval = convert(float(linkdict['mass']), units)
- mass.setAttribute("value", str(massval))
- inertial.appendChild(mass)
+ inertial.mass = massval
- 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]))
- inertia.setAttribute("ixy", str(matrix[1]))
- inertia.setAttribute("ixz", str(matrix[2]))
- inertia.setAttribute("iyy", str(matrix[4]))
- inertia.setAttribute("iyz", str(matrix[5]))
- inertia.setAttribute("izz", str(matrix[8]))
- inertial.appendChild(inertia)
+ inertial.matrix['ixx'] = matrix[0]
+ inertial.matrix['ixy'] = matrix[1]
+ inertial.matrix['ixz'] = matrix[2]
+ inertial.matrix['iyy'] = matrix[4]
+ inertial.matrix['iyz'] = matrix[5]
+ inertial.matrix['izz'] = matrix[8]
# 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))))
- iorigin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
- inertial.appendChild(iorigin)
+ inertial.origin = Pose(zero(off), zero(rpy))
- # 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))))
- origin.setAttribute("rpy", " ".join(map(str,zero(rpy))))
- visual.appendChild(origin)
- collision.appendChild(origin.cloneNode(1))
-
- # Define Material
- material = doc.createElement("material")
+ visual.origin = Pose(zero(off), zero(rpy))
+ collision.origin = visual.origin
+ # Define 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'])
- material.setAttribute("name", cname)
- visual.appendChild(material)
+ (cname, (r,g,b,a)) = self.getColor(linkdict['name'])
+
+ visual.material.name = cname
# If color has already been output, only output name
if not cname in self.usedcolors:
- color = doc.createElement("color")
- color.setAttribute("rgba", rgba)
- material.appendChild(color)
+ visual.material.color = Color(r,g,b,a)
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,
- and it's rgba. The color selected is based on the mesh name.
- If already seen, returns the saved color
- Otherwise, returns the next available color"""
+ and it's rgba. The color selected is based on the mesh name.
+ If already seen, returns the saved color
+ Otherwise, returns the next available color"""
if s in self.colormap:
return self.colormap[s]
color = COLORS[self.colorindex]
@@ -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'])
@@ -446,39 +414,28 @@ class Converter:
if parentname != pid:
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))
- joint.appendChild(limit)
+ setattr(limits, k, v)
+
if 'axis' in jointdict and jtype != 'fixed':
- axis = doc.createElement("axis")
- xyz = jointdict['axis'].replace(',', ' ')
- axis.setAttribute('xyz', xyz)
- joint.appendChild(axis)
+ axis = jointdict['axis'].replace(',', ' ')
# 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)
- rpy = zero(rpy)
- origin.setAttribute("xyz", " ".join(map(str,off)))
- origin.setAttribute("rpy", " ".join(map(str,rpy)))
- joint.appendChild(origin)
+ origin = Pose(zero(off), zero(rpy))
- 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
@@ -540,7 +497,7 @@ class Converter:
def groups(self, root):
""" For planning purposes, print out lists of
- all the links between the different joints"""
+ all the links between the different joints"""
self.groups = {}
self.makeGroup(root, "BASE")
s = ""
@@ -554,7 +511,7 @@ class Converter:
def makeGroup(self, id, gid):
""" Helper function for recursively gathering
- groups of joints. """
+ groups of joints. """
if gid in self.groups:
idlist = self.groups[gid]
idlist.append(id)
@@ -624,9 +581,9 @@ def matrixToQuaternion(matrix):
(R11, R12, R13, R21, R22, R23, R31, R32, R33) = matrix
# Build 4x4 matrix
M = [[R11, R21, R31, 0],
- [R12, R22, R32, 0],
- [R13, R23, R33, 0],
- [0, 0, 0, 1]]
+ [R12, R22, R32, 0],
+ [R13, R23, R33, 0],
+ [0, 0, 0, 1]]
A = numpy.array(M)
[w,x,y,z] = quaternion_from_matrix(A)
return [w,x,y,z]
@@ -646,48 +603,28 @@ def zero(arr):
return arr
-class TransformManager(threading.Thread):
- """Bridge to the ROS tf package. Constantly broadcasts
- all tfs, and retrieves them on demand."""
-
+class TransformManager:
def __init__(self):
- threading.Thread.__init__(self)
- self.running = True
- self.tfs = []
- self.listener = tf.TransformListener()
+ self.tf = tf.Transformer(True, rospy.Duration(10.0))
- def run(self):
- """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):
+ def add(self, offset, angle, parent, child):
"""Adds a new transform to the set"""
- tf = {'trans':trans, 'rot':rot, 'child':child, 'parent':parent}
- self.tfs.append(tf)
+ m = TransformStamped()
+ 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"""
- attempts = 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))
+ """Retrieves a transform"""
+ (off,rpy) = self.tf.lookupTransform(parent, child, rospy.Time(0))
+ return [list(off), list(rpy)]
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
diff --git a/simmechanics_to_urdf/manifest.xml b/simmechanics_to_urdf/manifest.xml
index 89bf15d..a1e4c61 100644
--- a/simmechanics_to_urdf/manifest.xml
+++ b/simmechanics_to_urdf/manifest.xml
@@ -9,6 +9,7 @@
+