def stl_to_mesh(filename, scale=(1, 1, 1)):
mesh = Mesh()
scene = pyassimp.load(filename)
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
pyassimp.release(scene)
return mesh
def __add_mesh(self, name, pose, filename, scale=(1, 1, 1)):
"""
This method is based on the __make_mesh() method of the moveit_commander.PlanningSceneInterface class
:return:
mesh: the mesh message corresponding to the mesh object
"""
co = moveit_msgs.msg.CollisionObject()
# pyassimp scene loading
if pyassimp is False:
raise moveit_commander.exception.MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt"
)
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise moveit_commander.exception.MoveItCommanderException(
"There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise moveit_commander.exception.MoveItCommanderException(
"There are no faces in the mesh")
co.operation = moveit_msgs.msg.CollisionObject.ADD
co.id = name
co.header = pose.header
# populating mesh message
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
else:
raise moveit_commander.exception.MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure"
)
for vertex in scene.meshes[0].vertices:
point = geometry_msgs.msg.Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
self.scene._pub_co.publish(co)
return mesh
def makeMesh(self, name, pose, filename):
if not use_pyassimp:
rospy.logerr(
'pyassimp is broken on your platform, cannot load meshes')
return
scene = pyassimp.load(filename)
if not scene.meshes:
rospy.logerr('Unable to load mesh')
return
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
pyassimp.release(scene)
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.meshes.append(mesh)
o.mesh_poses.append(pose)
o.operation = o.ADD
return o
def make_mesh(self, co, pose, filename, scale=(1.0, 1.0, 1.0)):
try:
scene = pyassimp.load(filename)
except AssimpError as e:
rospy.logerr("Assimp error: %s", e)
return False
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
if not isinstance(co.meshes, list):
co.meshes = []
co.meshes += [mesh]
if not isinstance(co.mesh_poses, list):
co.mesh_poses = []
co.mesh_poses += [pose.pose]
pyassimp.release(scene)
return co
def make_mesh(name, pose, filename, scale=(1, 1, 1)):
'''
returns a CollisionObject with the mesh defined in the path.
mostly copied from https://github.com/ros-planning/moveit_commander/blob/kinetic-devel/src/moveit_commander/planning_scene_interface.py
'''
print("Creating mesh with file from", filename)
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face) == 3:
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def makeMesh(self, name, pose, filename):
scene = pyassimp.load(filename)
if not scene.meshes:
rospy.logerr('Unable to load mesh')
return
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0],
face.indices[1],
face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
pyassimp.release(scene)
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.meshes.append(mesh)
o.mesh_poses.append(pose)
o.operation = o.ADD
return o
def __make_mesh(self, name, pose, filename):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def _get_meshes_from_pyassimp_node(node, scale):
meshes=[]
tf = node.transformation
# Match behavior of RViz for loading collada data
# See rviz mesh_loader.cpp buildMesh() lines 227-236
pnode = node.parent
while pnode is not None and hasattr(pnode, 'parent'):
# Don't convert to y-up orientation, which is what the root node in
# Assimp does
if pnode.parent is not None and hasattr(pnode.parent, 'parent'):
tf = np.dot(pnode.transformation, tf)
pnode = pnode.parent
for m in node.meshes:
mesh = Mesh()
for face in m.faces:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
for vertex in m.vertices:
point = Point()
vertex_h=np.dot(tf,np.hstack((vertex, [1])))
point.x = vertex_h[0] * scale[0]
point.y = vertex_h[1] * scale[1]
point.z = vertex_h[2] * scale[2]
mesh.vertices.append(point)
meshes.append(mesh)
for c in node.children:
meshes.extend( _get_meshes_from_pyassimp_node(c,scale))
return meshes
def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]*scale[0]
point.y = vertex[1]*scale[1]
point.z = vertex[2]*scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def _make_mesh(self, name, path, scale, pos, quat=None, frame_id=None):
if not use_pyassimp:
raise RuntimeError('pyassimp is broken, cannot load meshes')
scene = pyassimp.load(path)
if not scene.meshes:
pyassimp.release(scene)
raise RuntimeError('Unable to load mesh "{}"'.format(path))
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if isinstance(face, np.ndarray):
indices = face.tolist()
else:
indices = face.indices
if len(indices) == 3:
triangle.vertex_indices = list(indices)
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
pyassimp.release(scene)
return self._add_command(name,
self._make_pose(pos, quat),
mesh=mesh,
frame_id=frame_id)
def make_mesh(self, co, pose, filename, scale=(1.0, 1.0, 1.0)):
try:
scene = pyassimp.load(filename)
except AssimpError as e:
rospy.logerr("Assimp error: %s", e)
return False
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure"
)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
if not isinstance(co.meshes, list):
co.meshes = []
co.meshes += [mesh]
if not isinstance(co.mesh_poses, list):
co.mesh_poses = []
co.mesh_poses += [pose.pose]
pyassimp.release(scene)
return co
def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
co = CollisionObject()
if pyassimp is False:
raise MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt"
)
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, "__len__"):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, "indices"):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0],
face.indices[1],
face.indices[2],
]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure"
)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def make_mesh(co, name, pose, filename, scale=(1, 1, 1)):
#print("make_mesh(name=%s filename=%s)" % (name, filename))
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
if not isinstance(co.meshes, list):
co.meshes = []
co.meshes += [mesh]
if not isinstance(co.mesh_poses, list):
co.mesh_poses = []
co.mesh_poses += [pose.pose]
pyassimp.release(scene)
return co
def make_mesh(co, name, pose, filename, scale = (1, 1, 1)):
#print("make_mesh(name=%s filename=%s)" % (name, filename))
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face.indices[0],
face.indices[1],
face.indices[2]]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]*scale[0]
point.y = vertex[1]*scale[1]
point.z = vertex[2]*scale[2]
mesh.vertices.append(point)
if not isinstance(co.meshes, list):
co.meshes = []
co.meshes += [mesh]
if not isinstance(co.mesh_poses, list):
co.mesh_poses = []
co.mesh_poses += [pose.pose]
pyassimp.release(scene)
return co
def create_mesh(name,
pose,
filename,
scale=(1, 1, 1),
frame_id='/world_frame'):
co = CollisionObject()
scene = pyassimp.load(str(exo.Tools.parsePath(filename)))
if not scene.meshes or len(scene.meshes) == 0:
raise Exception("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise Exception("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header.frame_id = frame_id
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
else:
raise Exception("Unable to build triangles from mesh")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose]
pyassimp.release(scene)
return co
def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
co = CollisionObject()
if pyassimp is False:
raise MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt")
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face.indices[0],
face.indices[1],
face.indices[2]]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def to_shape_msgs_mesh(vertices, faces):
"""
Converts a set of vertices and faces to a ROS shape_msgs Mesh message
"""
mesh_msg = Mesh()
for face in faces:
triangle = MeshTriangle()
triangle.vertex_indices = face
mesh_msg.triangles.append(triangle)
for vertex in vertices:
pt = Point()
pt.x = vertex[0]
pt.y = vertex[1]
pt.z = vertex[2]
mesh_msg.vertices.append(pt)
return mesh_msg
def makeMesh(self, name, ps, filename):
"""
Make a mesh collision object
:param name: Name of the object
:param ps: A pose stamped object pose message
:param filename: The mesh file to load
"""
if not use_pyassimp:
rospy.logerr("pyassimp is broken on your platform, cannot load meshes")
return
scene = pyassimp.load(filename)
if not scene.meshes:
rospy.logerr("Unable to load mesh")
return
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [
face.indices[0],
face.indices[1],
face.indices[2],
]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
pyassimp.release(scene)
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = ps.header.frame_id
o.id = name
o.meshes.append(mesh)
o.mesh_poses.append(ps.pose)
o.operation = o.ADD
return o
