import time
from compas.datastructures import Mesh
import compas_fab
from compas_fab.backends import RosClient
from compas_fab.robots import CollisionMesh
from compas_fab.robots import PlanningScene
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
# create collison objects
mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
cm = CollisionMesh(mesh, 'tip')
# attach it to the end-effector
group = robot.main_group_name
scene.attach_collision_mesh_to_robot_end_effector(cm, group=group)
# sleep a bit before removing collision mesh
time.sleep(3)
scene.remove_attached_collision_mesh('tip')
scene.remove_collision_mesh('tip')
time.sleep(1)
T = Transformation.from_frame_to_frame(element0_tool0._tool_frame, tool.frame)
element0_tool0.transform(T)
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
# attach tool
robot.attach_tool(tool)
# add tool to scene
scene.add_attached_tool()
# create an attached collision mesh to the robot's end effector.
ee_link_name = robot.get_end_effector_link_name()
brick_acm = AttachedCollisionMesh(
CollisionMesh(element0_tool0.mesh, 'brick'), ee_link_name)
# add the collision mesh to the scene
scene.add_attached_collision_mesh(brick_acm)
time.sleep(2)
# Remove tool and brick
scene.remove_attached_collision_mesh(brick_acm.collision_mesh.id)
scene.remove_collision_mesh(brick_acm.collision_mesh.id)
scene.remove_attached_tool()
scene.remove_collision_mesh(tool.name)
robot.detach_tool()
time.sleep(1)