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 _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(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, 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 _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 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 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 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 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 to_Mesh(klampt_mesh): """From a Klampt Geometry3D or TriangleMesh to a ROS Mesh""" from klampt import Geometry3D,TriangleMesh from shape_msgs.msg import MeshTriangle if isinstance(klampt_mesh,Geometry3D): mesh = klampt_mesh.getTriangleMesh() T = klampt_mesh.getCurrentTransform() if T != se3.identity(): mesh.transform(*T) klampt_mesh = mesh ros_mesh = Mesh() for i in range(len(klampt_mesh.vertices)//3): k = i*3 ros_mesh.vertices.append(Vector3(klampt_mesh.vertices[k],klampt_mesh.vertices[k+1],klampt_mesh.vertices[k+2])) for i in range(len(klampt_mesh.indices)//3): k = i*3 ros_mesh.triangles.append(MeshTriangle([klampt_mesh.indices[k],klampt_mesh.indices[k+1],klampt_mesh.indices[k+2]])) return ros_mesh
def make_mesh(path, scale): """Load a mesh from disk and convert to a message. Arguments: path {str} -- path to the mesh file on disk scale {list} -- mesh scale x,y,z Returns: Mesh -- mesh message """ rospy.logdebug('Make mesh: %s, scale: %.3f,%.3f,%.3f', path, *scale) mesh = trimesh.load(path, force='mesh') return Mesh( vertices=[Point(*vertex) for vertex in mesh.vertices * scale], triangles=[ MeshTriangle(vertex_indices=indices) for indices in mesh.faces ])
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
def import_hull(xml_file): e = xml.etree.ElementTree.parse(xml_file).getroot() mesh = Mesh() for vertice_entry in e.findall('vertice'): vertice = Point() vertice.x = float(vertice_entry.get('x')) vertice.y = float(vertice_entry.get('y')) vertice.z = float(vertice_entry.get('z')) mesh.vertices.append(vertice) for triangle_entry in e.findall('triangle'): triangle = MeshTriangle() triangle.vertex_indices[0] = int(triangle_entry.get('id0')) triangle.vertex_indices[1] = int(triangle_entry.get('id1')) triangle.vertex_indices[2] = int(triangle_entry.get('id2')) mesh.triangles.append(triangle) return mesh
p.z = min_z box_co.meshes[0].vertices.append(p) p = Point() p.x = max_x p.y = max_y p.z = max_z box_co.meshes[0].vertices.append(p) p = Point() p.x = max_x p.y = max_y p.z = min_z box_co.meshes[0].vertices.append(p) t = MeshTriangle() t.vertex_indices[0] = 0 t.vertex_indices[1] = 1 t.vertex_indices[2] = 2 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 2 t.vertex_indices[1] = 3 t.vertex_indices[2] = 0 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 4 t.vertex_indices[1] = 3 t.vertex_indices[2] = 2
class ObjectBroadcaster: """ Given a detected object, request more information about it and publish the result as a CollisionObject """ def __init__(self, topic='/collision_object', diff_topic='/recognized_object_diff'): self._publisher = rospy.Publisher(topic, CollisionObject) self._diff_publisher = rospy.Publisher(diff_topic, PlanningScene) self._index = 1 self._min_confidence = 0.5 self._lock = Lock() self._get_object_info = rospy.ServiceProxy('get_object_info', GetObjectInformation) def broadcast_diff(self, objects): planning_scene = PlanningScene() planning_scene.is_diff = True if isinstance(objects, collections.Iterable): for ob in objects: co = self._create_collision_object(ob) planning_scene.world.collision_objects.append(co) else: co = self._create_collision_object(objects) planning_scene.world.collision_objects.append(co) self._diff_publisher.publish(planning_scene) def _create_collision_object(self, ob): if ob.confidence < self._min_confidence: rospy.loginfo( "Not publishing object of type %s because confidence %s < %s" % (ob.type.key, str(ob.confidence), str(self._min_confidence))) return info = None try: 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 min_y = 1000000 min_z = 1000000 max_x = -1000000 max_y = -1000000 max_z = -1000000 for v in co.meshes[0].vertices: if v.x > max_x: max_x = v.x if v.y > max_y: max_y = v.y if v.z > max_z: max_z = v.z if v.x < min_x: min_x = v.x if v.y < min_y: min_y = v.y if v.z < min_z: min_z = v.z box_co = copy.deepcopy(co) box_co.meshes[0].vertices = [] box_co.meshes[0].triangles = [] p = Point() p.x = min_x p.y = min_y p.z = min_z box_co.meshes[0].vertices.append(p) p = Point() p.x = max_x p.y = min_y p.z = min_z box_co.meshes[0].vertices.append(p) p = Point() p.x = max_x p.y = min_y p.z = max_z box_co.meshes[0].vertices.append(p) p = Point() p.x = min_x p.y = min_y p.z = max_z box_co.meshes[0].vertices.append(p) p = Point() p.x = min_x p.y = max_y p.z = max_z box_co.meshes[0].vertices.append(p) p = Point() p.x = min_x p.y = max_y p.z = min_z box_co.meshes[0].vertices.append(p) p = Point() p.x = max_x p.y = max_y p.z = max_z box_co.meshes[0].vertices.append(p) p = Point() p.x = max_x p.y = max_y p.z = min_z box_co.meshes[0].vertices.append(p) t = MeshTriangle() t.vertex_indices[0] = 0 t.vertex_indices[1] = 1 t.vertex_indices[2] = 2 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 2 t.vertex_indices[1] = 3 t.vertex_indices[2] = 0 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 4 t.vertex_indices[1] = 3 t.vertex_indices[2] = 2 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 2 t.vertex_indices[1] = 6 t.vertex_indices[2] = 4 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 7 t.vertex_indices[1] = 6 t.vertex_indices[2] = 2 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 2 t.vertex_indices[1] = 1 t.vertex_indices[2] = 7 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 3 t.vertex_indices[1] = 4 t.vertex_indices[2] = 5 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 5 t.vertex_indices[1] = 0 t.vertex_indices[2] = 3 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 0 t.vertex_indices[1] = 5 t.vertex_indices[2] = 7 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 7 t.vertex_indices[1] = 1 t.vertex_indices[2] = 0 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 7 t.vertex_indices[1] = 5 t.vertex_indices[2] = 4 box_co.meshes[0].triangles.append(t) t = MeshTriangle() t.vertex_indices[0] = 4 t.vertex_indices[1] = 6 t.vertex_indices[2] = 7 box_co.meshes[0].triangles.append(t) return box_co