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, 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: 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(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(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, 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 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 test_mesh_model_functions(): point1 = ROSPoint(0, 0, 0) point2 = ROSPoint(1, 0, 0) point3 = ROSPoint(1, 1, 0) point4 = ROSPoint(0, 1, 0) tri1 = ROSMeshTriangle() tri2 = ROSMeshTriangle() tri1.vertex_indices[0] = 0 tri1.vertex_indices[1] = 1 tri1.vertex_indices[2] = 2 tri2.vertex_indices[0] = 0 tri2.vertex_indices[1] = 2 tri2.vertex_indices[2] = 3 mesh3dmodel = TriangleMesh3DModel() mesh3dmodel.type = 'mesh3d' mesh3dmodel.geometry.vertices.append(point1) mesh3dmodel.geometry.vertices.append(point2) mesh3dmodel.geometry.vertices.append(point3) mesh3dmodel.geometry.vertices.append(point4) mesh3dmodel.geometry.triangles.append(tri1) mesh3dmodel.geometry.triangles.append(tri2) geo3dmodel = GeometryModel() geo3dmodel.fromTriangleMesh3DModel(mesh3dmodel) db().add(geo3dmodel) db().commit()
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 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 __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 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
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(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 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(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
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
def test_object_instance_insertion(): point2dmodel = Point2DModel() point2dmodel.type = '2dpoint' point2dmodel.geometry.x = 17.0 point2dmodel.geometry.y = 4.0 point2dmodel.geometry.z = 11.0 pose2dmodel = Pose2DModel() pose2dmodel.type = '2dpose' pose2dmodel.pose = ROSPose2D() pose2dmodel.pose.x = 0.5 pose2dmodel.pose.y = 1.5 pose2dmodel.pose.theta = 0.5 point0 = ROSPoint32(0, 0, 0) point1 = ROSPoint32(1, 0, 1) point2 = ROSPoint32(1, 1, 0) point3 = ROSPoint32(0, 1, 1) point4 = ROSPoint32(-1, 0, 1) point5 = ROSPoint32(-1, 1, 1) polygon0 = ROSPolygon() polygon0.points.append(point0) polygon0.points.append(point1) polygon0.points.append(point2) polygon0.points.append(point3) polygon1 = ROSPolygon() polygon1.points.append(point4) polygon1.points.append(point0) polygon1.points.append(point3) polygon1.points.append(point5) polygon2dmodel = Polygon2DModel() polygon2dmodel.type = '2dpolygon' polygon2dmodel.pose = ROSPose() polygon2dmodel.geometry = polygon0 point3dmodel = Point3DModel() point3dmodel.type = '3dpoint' point3dmodel.geometry.x = 19.0 point3dmodel.geometry.y = 66.0 point3dmodel.geometry.z = 12.0 pose3dmodel = Pose3DModel() pose3dmodel.type = '3dpose' pose3dmodel.pose = ROSPose() pose3dmodel.pose.position.x = 15.0 polygon3dmodel = Polygon3DModel() polygon3dmodel.type = '3dpolygon' polygon3dmodel.pose = ROSPose() polygon3dmodel.pose.position.x = 6.0 polygon3dmodel.geometry = polygon1 tri0 = ROSMeshTriangle() tri1 = ROSMeshTriangle() tri0.vertex_indices[0] = 0 tri0.vertex_indices[1] = 1 tri0.vertex_indices[2] = 2 tri1.vertex_indices[0] = 0 tri1.vertex_indices[1] = 2 tri1.vertex_indices[2] = 3 trianglemesh3dmodel = TriangleMesh3DModel() trianglemesh3dmodel.type = '3dtrianglemesh' trianglemesh3dmodel.pose = ROSPose() trianglemesh3dmodel.geometry.vertices.append(point0) trianglemesh3dmodel.geometry.vertices.append(point1) trianglemesh3dmodel.geometry.vertices.append(point2) trianglemesh3dmodel.geometry.vertices.append(point3) trianglemesh3dmodel.geometry.triangles.append(tri0) trianglemesh3dmodel.geometry.triangles.append(tri1) polygonmesh3dmodel = PolygonMesh3DModel() polygonmesh3dmodel.type = '3dpolygonmesh' polygonmesh3dmodel.pose = ROSPose() rand_quat = random_quaternion() polygonmesh3dmodel.pose.orientation.x = rand_quat[0] polygonmesh3dmodel.pose.orientation.y = rand_quat[1] polygonmesh3dmodel.pose.orientation.z = rand_quat[2] polygonmesh3dmodel.pose.orientation.w = rand_quat[3] polygonmesh3dmodel.geometry.polygons.append(polygon0) polygonmesh3dmodel.geometry.polygons.append(polygon1) desc_ros = ROSObjectDescription() desc_ros.type = "test_description" desc_ros.point2d_models.append(point2dmodel) desc_ros.pose2d_models.append(pose2dmodel) desc_ros.polygon2d_models.append(polygon2dmodel) desc_ros.point3d_models.append(point3dmodel) desc_ros.pose3d_models.append(pose3dmodel) desc_ros.polygon3d_models.append(polygon3dmodel) desc_ros.trianglemesh3d_models.append(trianglemesh3dmodel) desc_ros.polygonmesh3d_models.append(polygonmesh3dmodel) pose = ROSPoseStamped() pose.header.frame_id = "ref1" pose.pose.position.x = "0.0" pose.pose.position.y = "1.0" pose.pose.position.z = "0.0" pose.pose.orientation.x = "0.0" pose.pose.orientation.y = "0.0" pose.pose.orientation.z = "0.0" pose.pose.orientation.w = "1.0" inst_ros = ROSObjectInstance() inst_ros.alias = "mr_objecto" inst_ros.pose = pose inst_ros.description = desc_ros # create instance from ROS inst_db0 = ObjectInstance() inst_db0.fromROS(inst_ros) db().add(inst_db0) # create instance from existing model via id inst_db1 = ObjectInstance() inst_db1.object_description_id = 1 inst_db1.alias = "mrs_objecto" inst_db1.pose = GlobalPose() pose.pose.position.x = "1.0" pose.pose.position.y = "1.0" pose.pose.position.z = "0.0" inst_db1.pose.pose = fromROSPose(pose.pose) inst_db1.pose.ref_system = 'origin' #db().add(inst_db1) db().commit()
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
def create_complete_object_description(): point2dmodel = Point2DModel() point2dmodel.type = '2dpoint' point2dmodel.geometry.x = -0.5 point2dmodel.geometry.y = 0.0 point2dmodel.geometry.z = 0.0 pose2dmodel = Pose2DModel() pose2dmodel.type = '2dpose' pose2dmodel.pose = ROSPose2D() pose2dmodel.pose.x = 0.5 pose2dmodel.pose.y = 1.5 pose2dmodel.pose.theta = 1.5 point0 = ROSPoint32(0, 0, 0) point1 = ROSPoint32(1, 0, 0) point2 = ROSPoint32(1, 1, 0) point3 = ROSPoint32(0, 1, 0) point4 = ROSPoint32(-1, 0, 0.5) point5 = ROSPoint32(-1, 1, 0.5) point6 = ROSPoint32(1, 1, 0) point7 = ROSPoint32(2, 1, 0) point8 = ROSPoint32(2, 2, 0) point9 = ROSPoint32(1, 2, 0) point10 = ROSPoint32(-2, 1, 0) point11 = ROSPoint32(-2, 2, 0) polygon0 = ROSPolygon() polygon0.points.append(point0) polygon0.points.append(point1) polygon0.points.append(point2) polygon0.points.append(point3) polygon1 = ROSPolygon() polygon1.points.append(point4) polygon1.points.append(point0) polygon1.points.append(point3) polygon1.points.append(point5) polygon2 = ROSPolygon() polygon2.points.append(point6) polygon2.points.append(point7) polygon2.points.append(point8) polygon2.points.append(point9) polygon3 = ROSPolygon() polygon3.points.append(point10) polygon3.points.append(point6) polygon3.points.append(point9) polygon3.points.append(point11) polygon2dmodel = Polygon2DModel() polygon2dmodel.type = '2dpolygon' polygon2dmodel.pose = ROSPose() polygon2dmodel.geometry = polygon0 point3dmodel = Point3DModel() point3dmodel.type = '3dpoint' point3dmodel.geometry.x = 2.0 point3dmodel.geometry.y = 2.0 point3dmodel.geometry.z = 1.0 pose3dmodel = Pose3DModel() pose3dmodel.type = '3dpose' pose3dmodel.pose = ROSPose() pose3dmodel.pose.position.x = -0.5 pose3dmodel.pose.position.y = -0.5 pose3dmodel.pose.position.z = 0.5 rand_quat = random_quaternion() pose3dmodel.pose.orientation.x = rand_quat[0] pose3dmodel.pose.orientation.y = rand_quat[1] pose3dmodel.pose.orientation.z = rand_quat[2] pose3dmodel.pose.orientation.w = rand_quat[3] polygon3dmodel = Polygon3DModel() polygon3dmodel.type = '3dpolygon' polygon3dmodel.pose = ROSPose() polygon3dmodel.pose.position.x = 2.0 polygon3dmodel.geometry = polygon1 tri0 = ROSMeshTriangle() tri1 = ROSMeshTriangle() tri0.vertex_indices[0] = 0 tri0.vertex_indices[1] = 1 tri0.vertex_indices[2] = 2 tri1.vertex_indices[0] = 0 tri1.vertex_indices[1] = 2 tri1.vertex_indices[2] = 3 tpoint0 = ROSPoint( 0.0, 0.0, 0.0) tpoint1 = ROSPoint( 1.0, 0.0, 1.0) tpoint2 = ROSPoint( 1.0, 1.0, 0.0) tpoint3 = ROSPoint( 0.0, 1.0, 1.0) tpoint4 = ROSPoint(-1.0, 0.0, 0.0) tpoint5 = ROSPoint(-1.0, 1.0, 1.0) trianglemesh3dmodel = TriangleMesh3DModel() trianglemesh3dmodel.type = '3dtrianglemesh' trianglemesh3dmodel.pose = ROSPose() rand_quat = random_quaternion() trianglemesh3dmodel.pose.position.x = 1.0 trianglemesh3dmodel.pose.position.y = -2.0 trianglemesh3dmodel.pose.position.z = 0.25 trianglemesh3dmodel.pose.orientation.x = 0.0 trianglemesh3dmodel.pose.orientation.y = 0.0 trianglemesh3dmodel.pose.orientation.z = 0.0 trianglemesh3dmodel.pose.orientation.w = 0.0 trianglemesh3dmodel.geometry.vertices.append(tpoint0) trianglemesh3dmodel.geometry.vertices.append(tpoint1) trianglemesh3dmodel.geometry.vertices.append(tpoint2) trianglemesh3dmodel.geometry.vertices.append(tpoint3) trianglemesh3dmodel.geometry.triangles.append(tri0) trianglemesh3dmodel.geometry.triangles.append(tri1) polygonmesh3dmodel = PolygonMesh3DModel() polygonmesh3dmodel.type = '3dpolygonmesh' polygonmesh3dmodel.pose = ROSPose() rand_quat = random_quaternion() polygonmesh3dmodel.pose.position.x = 0.5 polygonmesh3dmodel.pose.position.y = 1.0 polygonmesh3dmodel.pose.position.z = 0.0 polygonmesh3dmodel.pose.orientation.x = 0.0 polygonmesh3dmodel.pose.orientation.y = 0.0 polygonmesh3dmodel.pose.orientation.z = 0.0 polygonmesh3dmodel.pose.orientation.w = 0.0 polygonmesh3dmodel.geometry.polygons.append(polygon2) polygonmesh3dmodel.geometry.polygons.append(polygon3) desc_ros = ROSObjectDescription() desc_ros.type = "Geometry" desc_ros.point2d_models.append(point2dmodel) desc_ros.pose2d_models.append(pose2dmodel) desc_ros.polygon2d_models.append(polygon2dmodel) desc_ros.point3d_models.append(point3dmodel) desc_ros.pose3d_models.append(pose3dmodel) desc_ros.polygon3d_models.append(polygon3dmodel) desc_ros.trianglemesh3d_models.append(trianglemesh3dmodel) desc_ros.polygonmesh3d_models.append(polygonmesh3dmodel) return desc_ros
from geometry_msgs.msg import Point from shape_msgs.msg import Mesh, MeshTriangle rospy.init_node('square_mesh') mesh = Mesh() for y in range(-1, 2, 2): for x in range(-1, 2, 2): pt = Point() pt.x = x pt.y = y pt.z = 0.0 mesh.vertices.append(pt) tri = MeshTriangle() tri.vertex_indices[0] = 0 tri.vertex_indices[1] = 1 tri.vertex_indices[2] = 2 mesh.triangles.append(tri) tri = MeshTriangle() tri.vertex_indices[0] = 1 tri.vertex_indices[1] = 3 tri.vertex_indices[2] = 2 mesh.triangles.append(tri) pub = rospy.Publisher("square_mesh", Mesh, queue_size=1) rate = rospy.Rate(1)