Example #1
0
    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)
Example #2
0
 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)
Example #3
0
 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
Example #4
0
 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