def remove_collision_mesh_async(self, callback, errback, id):
co = CollisionObject()
co.id = id
co.operation = CollisionObject.REMOVE
world = PlanningSceneWorld(collision_objects=[co])
scene = PlanningScene(world=world, is_diff=True)
request = scene.to_request(self.ros_client.ros_distro)
self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)
def add_collision_mesh_async(self, callback, errback, collision_mesh):
co = CollisionObject.from_collision_mesh(collision_mesh)
co.operation = CollisionObject.ADD
world = PlanningSceneWorld(collision_objects=[co])
scene = PlanningScene(world=world, is_diff=True)
request = scene.to_request(self.ros_client.ros_distro)
self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)
def build_collision_object(self, frame_id, id_name, compas_mesh,
operation):
co = CollisionObject(header=Header(frame_id=frame_id), id=id_name)
if compas_mesh:
# ROS mesh message requires triangles
mesh_quads_to_triangles(compas_mesh)
mesh = Mesh.from_mesh(compas_mesh)
co.meshes = [mesh]
co.mesh_poses = [Pose()]
if operation == 0:
co.operation = CollisionObject.ADD
elif operation == 1:
co.operation = CollisionObject.REMOVE
elif operation == 2:
co.operation = CollisionObject.APPEND
else:
raise ValueError("Operation unknown")
return co
def append_collision_mesh(self, collision_mesh):
"""Append a collision mesh to the planning scene."""
co = CollisionObject.from_collision_mesh(collision_mesh)
self._collision_object(co, CollisionObject.APPEND)
def remove_collision_mesh(self, id):
"""Remove a collision mesh from the planning scene."""
co = CollisionObject()
co.id = id
self._collision_object(co, CollisionObject.REMOVE)
def add_collision_mesh(self, collision_mesh):
"""Add a collision mesh to the planning scene."""
co = CollisionObject.from_collision_mesh(collision_mesh)
self._collision_object(co, CollisionObject.ADD)