def apply(self, state, **kwargs):
# TODO: identify surface automatically
with LockRenderer():
cone = get_viewcone(color=(1, 0, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration) # TODO: don't sleep if no viewer?
remove_body(cone)
wait_for_duration(1e-2)
if self.detect:
# TODO: the collision geometries are being visualized
# TODO: free the renderer
detections = get_visual_detections(self.robot,
camera_link=self.camera_frame)
print('Detections:', detections)
for body, dist in state.b_on.items():
obs = (body in detections) and (is_center_stable(
body, self.surface))
if obs or (self.surface not in state.task.rooms):
# TODO: make a command for scanning a room instead?
dist.obsUpdate(get_observation_fn(self.surface), obs)
#state.localized.update(detections)
# TODO: pose for each object that can be real or fake
yield
def apply(self, state, **kwargs):
with LockRenderer():
self.cone = get_viewcone(color=(1, 0, 0, 0.5))
state.poses[self.cone] = None
cone_pose = Pose(self.cone, unit_pose())
attach = Attach(self.robot, self.group, cone_pose, self.cone)
attach.assign()
wait_for_duration(1e-2)
for _ in attach.apply(state, **kwargs):
yield
def apply(self, state, **kwargs):
# TODO: identify surface automatically
cone = get_viewcone(color=(1, 0, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(self._duration) # TODO: don't sleep if no viewer?
remove_body(cone)
detections = get_visual_detections(self.robot)
print('Detections:', detections)
for body, dist in state.b_on.items():
obs = (body in detections) and (is_center_stable(
body, self.surface))
if obs or (self.surface not in state.task.rooms):
# TODO: make a command for scanning a room instead?
dist.obsUpdate(get_observation_fn(self.surface), obs)
def apply(self, state, **kwargs):
self.cone = get_viewcone(color=(1, 0, 0, 0.5))
state.poses[self.cone] = None
attach = Attach(self.robot, self.group, Pose(self.cone, unit_pose()),
self.cone)
attach.apply(state, **kwargs)
