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()
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
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)