def gen(rover, objective):
base_joints = get_base_joints(rover)
target_point = get_point(objective)
base_generator = visible_base_generator(rover, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
attempts = 0
while True:
if max_attempts <= attempts:
attempts = 0
yield None
attempts += 1
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
link_pose = get_link_pose(rover,
link_from_name(rover, KINECT_FRAME))
if use_cone:
mesh, _ = get_detection_cone(rover,
objective,
camera_link=KINECT_FRAME,
depth=max_range)
if mesh is None:
continue
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
local_pose = Pose()
else:
distance = get_distance(point_from_pose(link_pose),
target_point)
if max_range < distance:
continue
cone = create_cylinder(radius=0.01,
height=distance,
color=(0, 0, 1, 0.5))
local_pose = Pose(Point(z=distance / 2.))
set_pose(cone, multiply(link_pose, local_pose))
# TODO: colors corresponding to scanned object
if any(
pairwise_collision(cone, b) for b in obstacles
if b != objective and not is_placement(objective, b)):
# TODO: ensure that this works for the rover
remove_body(cone)
continue
if not reachable_test(rover, bq):
continue
print('Visibility attempts:', attempts)
y = Ray(cone, rover, objective)
yield Output(bq, y)
def apply(self, state, **kwargs):
mesh, _ = get_detection_cone(self.robot,
self.body,
depth=MAX_KINECT_DISTANCE)
assert (mesh is not None)
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(self._duration)
# time.sleep(1.0)
remove_body(cone)
state.registered.add(self.body)
def apply(self, state, **kwargs):
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)
with LockRenderer():
cone = create_mesh(mesh, color=(0, 1, 0, 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
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
