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(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 __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 __make_mesh_custom(self, name, pose, mesh):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
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 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
rospy.logwarn(
"Unable to retrieve object information for object of type\n%s"
% str(ob.type))
co = CollisionObject()
co.type = ob.type
co.operation = CollisionObject.ADD
co.header = ob.pose.header
if info:
if len(info.name) > 0:
co.id = info.name + '_' + str(self._bump_index())
else:
co.id = ob.type.key + '_' + str(self._bump_index())
if len(info.ground_truth_mesh.triangles) > 0:
co.meshes = [info.ground_truth_mesh]
else:
co.meshes = [ob.bounding_mesh]
co.mesh_poses = [ob.pose.pose.pose]
else:
rospy.loginfo("Did not find information for object %s:" %
(ob.type.key))
co.id = ob.type.key + '_' + str(self._bump_index())
co.meshes = [ob.bounding_mesh]
co.mesh_poses = [ob.pose.pose.pose]
if len(co.meshes[0].triangles) > 0:
rospy.loginfo("Publishing collision object %s with confidence %s" %
(co.id, str(ob.confidence)))
# hack to turn the mesh into a box (aabb)
#co.primitive_poses = co.mesh_poses
info = self._get_object_info(ob.type).information
except rospy.ServiceException, e:
rospy.logwarn("Unable to retrieve object information for object of type\n%s" % str(ob.type))
co = CollisionObject()
co.type = ob.type
co.operation = CollisionObject.ADD
co.header = ob.pose.header
if info:
if len(info.name) > 0:
co.id = info.name + "_" + str(self._bump_index())
else:
co.id = ob.type.key + "_" + str(self._bump_index())
if len(info.ground_truth_mesh.triangles) > 0:
co.meshes = [info.ground_truth_mesh]
else:
co.meshes = [ob.bounding_mesh]
co.mesh_poses = [ob.pose.pose.pose]
else:
rospy.loginfo("Did not find information for object %s:" % (ob.type.key))
co.id = ob.type.key + "_" + str(self._bump_index())
co.meshes = [ob.bounding_mesh]
co.mesh_poses = [ob.pose.pose.pose]
if len(co.meshes[0].triangles) > 0:
rospy.loginfo("Publishing collision object %s with confidence %s" % (co.id, str(ob.confidence)))
# hack to turn the mesh into a box (aabb)
# co.primitive_poses = co.mesh_poses
# co.mesh_poses = []
min_x = 1000000
