Exemple #1
0
def enter_edit_mode(client: airsim.MultirotorClient,
                    points: List[Vector3r]) -> None:
    mode = EditMode.TRANSLATING
    step = 1.0
    total = {
        EditMode.TRANSLATING:
        Vector3r(0, 0, 0),
        # EditMode.ROTATING: Vector3r(0, 0, 0),
        EditMode.SCALING:
        Vector3r(1, 1, 1),
    }
    try:
        while True:
            key = ord(airsim.wait_key())
            if key == 224:  # arrow keys
                arrow_key = ArrowKey(ord(airsim.wait_key()))
                client.simFlushPersistentMarkers()
                if mode == EditMode.TRANSLATING:
                    delta = {
                        ArrowKey.Up: Vector3r(step, 0, 0),
                        ArrowKey.Down: Vector3r(-step, 0, 0),
                        ArrowKey.Left: Vector3r(0, -step, 0),
                        ArrowKey.Right: Vector3r(0, step, 0),
                    }[arrow_key]
                    points = [point + delta for point in points]
                    total[EditMode.TRANSLATING] += delta
                elif mode == EditMode.SCALING:
                    factor = {
                        ArrowKey.Up: 1 + step * 0.1,
                        ArrowKey.Down: 1 - step * 0.1,
                        ArrowKey.Left: 1 - step * 0.1,
                        ArrowKey.Right: 1 + step * 0.1,
                    }[arrow_key]
                    points = [point * factor for point in points]
                    total[EditMode.SCALING] *= factor
                else:
                    assert False  # TODO handle other edit modes for rotating and scaling
                client.simPlotPoints(points,
                                     Rgba.Blue,
                                     POINT_CLOUD_POINT_SIZE,
                                     is_persistent=True)
            elif key == 27:  # esc
                ff.log("TRANSLATING:", total[EditMode.TRANSLATING])
                ff.log("SCALING:", total[EditMode.SCALING])
                return
            else:
                if key == ord(b"["):
                    step /= 2.0
                elif key == ord(b"]"):
                    step *= 2.0
                elif key == ord(b"\t"):
                    mode = EditMode.next(mode)
                ff.log(f"{mode=} {step=}")
    except KeyboardInterrupt:
        return
Exemple #2
0
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
    initial_pose = client.simGetVehiclePose()
    if args.verbose:
        ff.print_pose(initial_pose, airsim.to_eularian_angles)

    if args.flush:
        client.simFlushPersistentMarkers()

    # NOTE we have to use (-y, -x, -z), and not (x, -y, -z), because these
    # files were exported from Unreal Engine as FBX with Force Front XAxis
    BUILDING = False
    STATUE = True

    def convert_position(p, t, s):
        if BUILDING or STATUE:
            if t is not None: p += t
            if s is not None: p *= s
            x, y, z = map(float, p)
            return Vector3r(-y, -x, -z)
        else:
            return convert_uavmvs_to_airsim_position(p, t, s)

    points = [
        # convert_uavmvs_to_airsim_position(_, translation=args.offset, scaling=args.scale)
        convert_position(_, args.offset, args.scale)
        for _ in args.points[::args.every_k]
    ]
    client.simPlotPoints(points,
                         Rgba.Blue,
                         POINT_CLOUD_POINT_SIZE,
                         is_persistent=True)

    if args.trajectory is not None:
        camera_poses, camera_positions = [], []
        for i, camera in enumerate(args.trajectory):
            if not camera.spline_interpolated:  # XXX
                pose = convert_uavmvs_to_airsim_pose(camera=camera,
                                                     translation=args.offset,
                                                     scaling=args.scale)
                # print(f"=== {i} ===")
                # print(camera)
                # print(pose)
                camera_poses.append(pose)
                camera_positions.append(pose.position)

        # client.simPlotLineStrip(camera_positions, Rgba.Black, TRAJECTORY_THICKNESS, is_persistent=True)
        # client.simPlotPoints(camera_positions, Rgba.White, CAMERA_POSE_SIZE, is_persistent=True)
        client.simPlotTransforms(camera_poses,
                                 10 * CAMERA_POSE_SIZE,
                                 is_persistent=True)

    if args.edit:
        enter_edit_mode(client, points)
Exemple #3
0
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
    if args.flush:
        client.simFlushPersistentMarkers()

    if not args.transformation:
        poses = [
            Pose(record.position, record.orientation)
            for record in args.recording.values()
        ]
        positions = [record.position for record in args.recording.values()]
    else:
        matrix = np.loadtxt(
            args.transformation)  # load the 4x4 transformation matrix
        print(matrix)
        poses = []
        positions = []
        for record in args.recording.values():
            pos = Vector3r(*np.matmul(matrix, np.append(record.position, 1)))
            poses.append(Pose(pos, record.orientation))
            positions = [pos]

    if args.axes:
        client.simPlotTransforms(poses,
                                 scale=100.0,
                                 thickness=2.5,
                                 is_persistent=True)
    else:
        client.simPlotPoints(positions,
                             args.color,
                             size=10,
                             is_persistent=True)

    if args.lines:
        client.simPlotLineStrip(positions,
                                args.color,
                                thickness=2.5,
                                is_persistent=True)

    if args.aim:
        line_list = []
        for pose in poses:
            line_list.append(pose.position)
            x_axis, _, _ = AirSimNedTransform.local_axes_frame(pose)
            line_list.append(pose.position + x_axis * 100)
        client.simPlotLineList(line_list,
                               args.color,
                               thickness=2.5,
                               is_persistent=True)
Exemple #4
0
def fly_zone(client: airsim.MultirotorClient,
             zone: Rect,
             altitude_shift: float = 0.0) -> None:
    path = [
        Vector3r(corner.x_val, corner.y_val, corner.z_val - altitude_shift)
        for corner in zone.corners(repeat_first=True)
    ]
    if AUGMENT_PATHS:
        path = Controller.augment_path(path, max_dist=2)

    if SHOW_PLOTS:
        client.simPlotPoints(points=path,
                             is_persistent=True,
                             color_rgba=Rgba.White,
                             size=5)
        client.simPlotLineStrip(points=path,
                                is_persistent=True,
                                color_rgba=Rgba.White)
    # Controller.fly_path(client, path) ##client.moveOnPathAsync(path, velocity=2).join()

    # XXX testing.. stretching Rect, zigzagging path and flying over it
    zz_path = Rect(
        Vector3r(0, 0, -altitude_shift) + zone.center,
        # Vector3r(0, 0, -altitude_shift) + zone.half_width * 4,
        # Vector3r(0, 0, -altitude_shift) + zone.half_height * 4,
        zone.half_width * 2,
        zone.half_height * 2,
    ).zigzag(4)
    if AUGMENT_PATHS:
        zz_path = Controller.augment_path(zz_path, max_dist=2)

    if SHOW_PLOTS:
        client.simPlotPoints(points=zz_path,
                             is_persistent=True,
                             color_rgba=Rgba.White,
                             size=5)
        client.simPlotLineStrip(points=zz_path,
                                is_persistent=True,
                                color_rgba=Rgba.Green)
    Controller.fly_path(
        client, zz_path)  ##client.moveOnPathAsync(zz_path, velocity=2).join()
Exemple #5
0
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
    if args.flush:
        client.simFlushPersistentMarkers()

    def pose_from_meshroom_to_airsim(meshroom_pose):
        assert len(meshroom_pose.center) == 3 and len(
            meshroom_pose.rotation) == 9, meshroom_pose
        xyzw = MeshroomTransform.rotation(meshroom_pose.rotation,
                                          as_xyzw_quaternion=True)
        return Pose(Vector3r(*meshroom_pose.center), Quaternionr(*xyzw))

    poses = [
        pose_from_meshroom_to_airsim(pose)
        for pose in args.poses_dict.values()
    ]
    positions = [pose.position for pose in poses]

    if args.axes:
        client.simPlotTransforms(poses,
                                 scale=75.0,
                                 thickness=2.5,
                                 is_persistent=True)
    else:
        client.simPlotPoints(positions,
                             args.color,
                             size=10,
                             is_persistent=True)

    if args.lines:
        client.simPlotLineStrip(positions,
                                args.color,
                                thickness=2.5,
                                is_persistent=True)

    if args.transformation:
        meshroom_to_airsim = np.loadtxt(
            args.transformation)  # load the 4x4 transformation matrix
        print(meshroom_to_airsim)

        def align_meshroom_to_airsim(meshroom_pose,
                                     meshroom_to_airsim_transform):
            # NOTE this transformation is only based on the positions (and not on the orientations)
            meshroom_pos = np.append(meshroom_pose.position.to_numpy_array(),
                                     1)  # [x, y, z, 1]
            airsim_pos = np.matmul(meshroom_to_airsim_transform, meshroom_pos)
            return Pose(Vector3r(*airsim_pos), meshroom_pose.orientation)

        aligned_poses = [
            align_meshroom_to_airsim(pose, meshroom_to_airsim)
            for pose in poses
        ]
        aligned_positions = [pose.position for pose in aligned_poses]

        if args.axes:
            client.simPlotTransforms(aligned_poses,
                                     scale=75.0,
                                     thickness=2.5,
                                     is_persistent=True)
        else:
            client.simPlotPoints(aligned_positions,
                                 args.color,
                                 size=10,
                                 is_persistent=True)

        if args.lines:
            client.simPlotLineStrip(positions,
                                    args.color,
                                    thickness=2.5,
                                    is_persistent=True)
Exemple #6
0
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
    initial_pose = client.simGetVehiclePose()
    if args.verbose:
        ff.print_pose(initial_pose, airsim.to_eularian_angles)
        ff.log(client.simGetCameraInfo(camera_name=CAPTURE_CAMERA))

    if args.flush or (args.capture_dir and not args.debug):
        client.simFlushPersistentMarkers()

    camera_poses = []
    for camera in args.trajectory:
        position = convert_uavmvs_to_airsim_position(
            camera.position, translation=args.offset, scaling=args.scale
        )
        orientation = quaternion_orientation_from_eye_to_look_at(position, LOOK_AT_TARGET)
        camera_poses.append(Pose(position, orientation))

    if args.debug:
        camera_positions = [pose.position for pose in camera_poses]
        client.simPlotPoints(camera_positions, Rgba.Blue, is_persistent=True)
        client.simPlotLineStrip(camera_positions, Rgba.Cyan, thickness=2.5, is_persistent=True)

    airsim_record = []

    def do_stuff_at_uavmvs_viewpoint(i, pose):
        nonlocal client, camera_poses, airsim_record
        log_string = f"({i}/{len(camera_poses)})"
        p, q = pose.position, pose.orientation
        if args.debug:
            log_string += f" position = {to_xyz_str(p)}"
            client.simPlotTransforms([pose], scale=100, is_persistent=True)
            # client.simPlotArrows([p], [LOOK_AT_TARGET], Rgba.White, thickness=3.0, duration=10)
        elif args.capture_dir:
            path = f"{args.prefix}pose{args.suffix}_{i:0{len(str(len(camera_poses)))}}.png"
            path = os.path.join(args.capture_dir, path)
            airsim.write_png(path, AirSimImage.get_mono(client, CAPTURE_CAMERA))
            log_string += f' saved image to "{path}"'
            record_line = AirSimRecord.make_line_string(p, q, time_stamp=str(i), image_file=path)
            airsim_record.append(record_line)
        ff.log(log_string)

    if IS_CV_MODE:
        for i, camera_pose in enumerate(camera_poses):
            client.simSetVehiclePose(camera_pose, ignore_collision=True)
            do_stuff_at_uavmvs_viewpoint(i, camera_pose)
            time.sleep(CV_SLEEP_SEC)
    else:
        client.moveToZAsync(z=-10, velocity=VELOCITY).join()  # XXX avoid colliding on take off
        client.hoverAsync().join()
        mean_position_error = 0.0

        for i, camera_pose in enumerate(camera_poses):
            client.moveToPositionAsync(
                *to_xyz_tuple(camera_pose.position),
                velocity=VELOCITY,
                drivetrain=DrivetrainType.MaxDegreeOfFreedom,
                yaw_mode=YawMode(is_rate=False, yaw_or_rate=YAW_N),
            ).join()

            with pose_at_simulation_pause(client) as real_pose:
                # NOTE when we pre-compute the viewpoint's camera orientation, we use the
                # expected drone position, which (should be close, but) is not the actual
                # drone position. Hence, we could experiment with using fake orientation:
                # quaternion_orientation_from_eye_to_look_at(real_pose.position, LOOK_AT_TARGET)
                fake_pose = Pose(real_pose.position, camera_pose.orientation)

                client.simSetVehiclePose(fake_pose, ignore_collision=True)
                client.simContinueForFrames(1)  # NOTE ensures pose change
                do_stuff_at_uavmvs_viewpoint(i, fake_pose)
                client.simSetVehiclePose(real_pose, ignore_collision=True)

                position_error = real_pose.position.distance_to(camera_pose.position)
                mean_position_error += position_error
                ff.log_debug(f"{position_error = }")

        mean_position_error /= len(camera_poses)
        ff.log_debug(f"{mean_position_error = }")

    if airsim_record:
        print_to_file = args.record_path is not None
        file = open(args.record_path, "w") if print_to_file else None
        print(AirSimRecord.make_header_string(), file=(file if print_to_file else sys.stdout))
        for line in airsim_record:
            print(line, file=(file if print_to_file else sys.stdout))
        if print_to_file:
            file.close()
            ff.log_info(f'Saved AirSim record to "{args.record_path}"')