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