def from_msg(cls, msg):
kwargs = {}
kwargs['header'] = Header.from_msg(msg['header'])
kwargs['id'] = msg['id']
kwargs['type'] = ObjectType.from_msg(msg['type'])
kwargs['pose'] = Pose.from_msg(
msg['pose']) if 'pose' in msg else Pose()
kwargs['primitives'] = [
SolidPrimitive.from_msg(i) for i in msg['primitives']
]
kwargs['primitive_poses'] = [
Pose.from_msg(i) for i in msg['primitive_poses']
]
kwargs['meshes'] = [Mesh.from_msg(i) for i in msg['meshes']]
kwargs['mesh_poses'] = [Pose.from_msg(i) for i in msg['mesh_poses']]
kwargs['planes'] = [Plane.from_msg(i) for i in msg['planes']]
kwargs['plane_poses'] = [
Pose.from_frame(i) for i in msg['plane_poses']
]
kwargs['operation'] = msg['operation']
return cls(**kwargs)
def from_collision_mesh(cls, collision_mesh):
"""Creates a collision object from a :class:`compas_fab.robots.CollisionMesh`
"""
kwargs = {}
kwargs['header'] = Header(frame_id=collision_mesh.root_name)
kwargs['id'] = collision_mesh.id
kwargs['meshes'] = [Mesh.from_mesh(collision_mesh.mesh)]
kwargs['mesh_poses'] = [Pose.from_frame(collision_mesh.frame)]
kwargs['pose'] = Pose()
return cls(**kwargs)
def __init__(self,
header=None,
id="collision_obj",
type=None,
primitives=None,
primitive_poses=None,
meshes=None,
mesh_poses=None,
planes=None,
plane_poses=None,
subframe_names=None,
subframe_poses=None,
operation=0,
pose=None):
self.header = header or Header(
) # a header, used for interpreting the poses
self.id = id # the id of the object (name used in MoveIt)
self.type = type or ObjectType(
) # The object type in a database of known objects
self.pose = pose or Pose(
) # currently not actively used in FAB, but needed to be present otherwise ROS Noetic complains about empty quaternion
# solid geometric primitives
self.primitives = primitives or []
self.primitive_poses = primitive_poses or []
# meshes
self.meshes = meshes or []
self.mesh_poses = mesh_poses or []
# bounding planes
self.planes = planes or []
self.plane_poses = plane_poses or []
self.operation = operation # ADD or REMOVE or APPEND or MOVE
def to_collision_meshes(self):
"""Creates a list of collision meshes from a :class:`compas_fab.backends.CollisionObject`
"""
collision_meshes = []
for mesh, pose in zip(self.meshes, self.mesh_poses):
pose = pose if isinstance(pose, Pose) else Pose(**pose)
pose.position = pose.position if isinstance(pose.position, Point) else Point(**pose.position)
pose.position.x = float(pose.position.x)
pose.position.y = float(pose.position.y)
pose.position.z = float(pose.position.z)
pose.orientation = pose.orientation if isinstance(pose.orientation, Quaternion) else Quaternion(**pose.orientation)
pose.orientation.x = float(pose.orientation.x)
pose.orientation.y = float(pose.orientation.y)
pose.orientation.z = float(pose.orientation.z)
pose.orientation.w = float(pose.orientation.w)
mesh = mesh if isinstance(mesh, Mesh) else Mesh(**mesh)
mesh.triangles = [t if isinstance(t, MeshTriangle) else MeshTriangle(**t) for t in mesh.triangles]
for triangle in mesh.triangles:
triangle.vertex_indices = [int(x) for x in triangle.vertex_indices]
mesh.vertices = [v if isinstance(v, Point) else Point(**v) for v in mesh.vertices]
for vertex in mesh.vertices:
vertex.x = float(vertex.x)
vertex.y = float(vertex.y)
vertex.z = float(vertex.z)
root_name = getattr(self.header, 'frame_id', None) or self.header['frame_id']
cm = CollisionMesh(mesh.mesh, self.id, pose.frame, root_name)
collision_meshes.append(cm)
return collision_meshes
def from_mesh(cls, mesh):
"""Creates a `BoundingVolume` from a :class:`compas.datastructures.Mesh`.
Parameters
----------
sphere: `compas.datastructures.Mesh`
"""
mesh = Mesh.from_mesh(mesh)
pose = Pose()
return cls(meshes=[mesh], mesh_poses=[pose])
def from_sphere(cls, sphere):
"""Creates a `BoundingVolume` from a :class:`compas.geometry.Sphere`.
Parameters
----------
sphere: `compas.geometry.Sphere`
"""
primitive = SolidPrimitive.from_sphere(sphere)
pose = Pose(Point(*sphere.point), Quaternion(0, 0, 0, 1))
return cls(primitives=[primitive], primitive_poses=[pose])
def from_box(cls, box):
"""Creates a `BoundingVolume` from a :class:`compas.geometry.Box`.
Parameters
----------
box: `compas.geometry.Box`
"""
primitive = SolidPrimitive.from_box(box)
pose = Pose.from_frame(box.frame)
return cls(primitives=[primitive], primitive_poses=[pose])