def apply(self, state, **kwargs):
cone = self.attach.cone
detach = Detach(self.attach.robot, self.attach.group, cone)
for _ in detach.apply(state, **kwargs):
yield
del state.poses[cone]
remove_body(cone)
wait_for_duration(1e-2)
def apply(self, state, **kwargs):
print(self.visual_data)
with LockRenderer():
visual_id = visual_shape_from_data(self.visual_data[0])
cone = create_body(visual_id=visual_id)
#cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
set_pose(cone, self.pose)
wait_for_duration(self._duration)
with LockRenderer():
remove_body(cone)
wait_for_duration(1e-2)
wait_for_duration(self._duration)
# TODO: set to transparent before removing
yield
def apply(self, state, **kwargs):
# TODO: check if actually can register
mesh, _ = get_detection_cone(self.robot,
self.body,
camera_link=self.camera_frame,
depth=self.max_depth)
if mesh is None:
wait_for_user()
assert (
mesh is not None
) # TODO: is_visible_aabb(body_aabb, **kwargs)=False sometimes without collisions
with LockRenderer():
cone = create_mesh(mesh, color=apply_alpha(GREEN, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration)
remove_body(cone)
wait_for_duration(1e-2)
state.registered.add(self.body)
yield
