Extend a map, display it¶

// The QiContext provided by the QiSDK.
private QiContext qiContext;

// Store the provided QiContext.
this.qiContext = qiContext;

// Remove the QiContext.
this.qiContext = null;

private Future<ExplorationMap> mapSurroundings(QiContext qiContext) {
    // Create a Promise to set the operation state later.
    Promise<ExplorationMap> promise = new Promise<>();
    // If something tries to cancel the associated Future, do cancel it.
    promise.setOnCancel(ignored -> {
        if (!promise.getFuture().isDone()) {
            promise.setCancelled();
        }
    });

    // Create a LocalizeAndMap, run it, and keep the Future.
    Future<Void> localizeAndMapFuture = LocalizeAndMapBuilder.with(qiContext)
            .buildAsync()
            .andThenCompose(localizeAndMap -> {
                // Add an OnStatusChangedListener to know when the robot is localized.
                localizeAndMap.addOnStatusChangedListener(status -> {
                    if (status == LocalizationStatus.LOCALIZED) {
                        // Retrieve the map.
                        ExplorationMap explorationMap = localizeAndMap.dumpMap();
                        // Set the Promise state in success, with the ExplorationMap.
                        if (!promise.getFuture().isDone()) {
                            promise.setValue(explorationMap);
                        }
                    }
                });

                // Run the LocalizeAndMap.
                return localizeAndMap.async().run().thenConsume(future -> {
                    // Remove the OnStatusChangedListener.
                    localizeAndMap.removeAllOnStatusChangedListeners();
                    // In case of error, forward it to the Promise.
                    if (future.hasError() && !promise.getFuture().isDone()) {
                        promise.setError(future.getErrorMessage());
                    }
                });
            });

    // Return the Future associated to the Promise.
    return promise.getFuture().thenCompose(future -> {
        // Stop the LocalizeAndMap.
        localizeAndMapFuture.cancel(true);
        return future;
    });
}

// The initial ExplorationMap.
private ExplorationMap initialExplorationMap = null;

private Bitmap mapToBitmap(ExplorationMap explorationMap) {
    // Get the ByteBuffer containing the map graphical representation.
    ByteBuffer byteBuffer = explorationMap.getTopGraphicalRepresentation().getImage().getData();
    byteBuffer.rewind();
    // Get the buffer size.
    int size = byteBuffer.remaining();
    // Transform the buffer to a ByteArray.
    byte[] byteArray = new byte[size];
    byteBuffer.get(byteArray);
    // Transform the ByteArray to a Bitmap.
    return BitmapFactory.decodeByteArray(byteArray, 0, size);
}

private void displayMap(Bitmap bitmap) {
    // Set the ImageView bitmap.
    mapImageView.setImageBitmap(bitmap);
}

private void startMappingStep(QiContext qiContext) {
    // Disable "start mapping" button.
    startMappingButton.setEnabled(false);
    // Map the surroundings and get the map.
    mapSurroundings(qiContext).thenConsume(future -> {
        if (future.isSuccess()) {
            ExplorationMap explorationMap = future.get();
            // Store the initial map.
            this.initialExplorationMap = explorationMap;
            // Convert the map to a bitmap.
            Bitmap bitmap = mapToBitmap(explorationMap);
            // Display the bitmap and enable "extend map" button.
            runOnUiThread(() -> {
                if (getLifecycle().getCurrentState().isAtLeast(Lifecycle.State.RESUMED)) {
                    displayMap(bitmap);
                    extendMapButton.setEnabled(true);
                }
            });
        } else {
            // If the operation is not a success, re-enable "start mapping" button.
            runOnUiThread(() -> {
                if (getLifecycle().getCurrentState().isAtLeast(Lifecycle.State.RESUMED)) {
                    startMappingButton.setEnabled(true);
                }
            });
        }
    });
}

// Set the button onClick listener.
startMappingButton.setOnClickListener(ignored -> {
    // Check that the Activity owns the focus.
    if(qiContext == null) return;
    // Start the mapping step.
    startMappingStep(qiContext);
});

@Override
protected void onResume() {
    super.onResume();
    // Reset UI and variables state.
    startMappingButton.setEnabled(false);
    extendMapButton.setEnabled(false);
    initialExplorationMap = null;
    mapImageView.setImageBitmap(null);
}

// Enable "start mapping" button.
runOnUiThread(() -> startMappingButton.setEnabled(true));

private Future<Void> publishExplorationMap(LocalizeAndMap localizeAndMap, Consumer<ExplorationMap> updatedMapCallback) {
    // Retrieve the map.
    return localizeAndMap.async().dumpMap().andThenCompose(explorationMap -> {
        // Call the callback with the map.
        updatedMapCallback.consume(explorationMap);
        // Wait for 2 seconds.
        return FutureUtils.wait(2L, TimeUnit.SECONDS);
    }).andThenCompose(ignored -> {
        // Call the method recursively.
        return publishExplorationMap(localizeAndMap, updatedMapCallback);
    });
}

private Future<Void> extendMap(ExplorationMap explorationMap, QiContext qiContext, Consumer<ExplorationMap> updatedMapCallback) {
    // Create a Promise to set the operation state later.
    Promise<Void> promise = new Promise<>();
    // If something tries to cancel the associated Future, do cancel it.
    promise.setOnCancel(ignored -> {
        if (!promise.getFuture().isDone()) {
            promise.setCancelled();
        }
    });

    // Create a LocalizeAndMap with the initial map, run it, and keep the Future.
    Future<Void> localizeAndMapFuture = LocalizeAndMapBuilder.with(qiContext)
            .withMap(explorationMap)
            .buildAsync()
            .andThenCompose(localizeAndMap -> {
                // Create a Future for map notification.
                final Future<Void>[] publishExplorationMapFuture = new Future[]{null};

                // Add an OnStatusChangedListener to know when the robot is localized.
                localizeAndMap.addOnStatusChangedListener(status -> {
                    if (status == LocalizationStatus.LOCALIZED) {
                        // Start the map notification process.
                        publishExplorationMapFuture[0] = publishExplorationMap(localizeAndMap, updatedMapCallback);
                    }
                });

                // Run the LocalizeAndMap.
                return localizeAndMap.async().run().thenConsume(future -> {
                    // Remove the OnStatusChangedListener.
                    localizeAndMap.removeAllOnStatusChangedListeners();
                    // Stop the map notification process.
                    if (publishExplorationMapFuture[0] != null) {
                        publishExplorationMapFuture[0].cancel(true);
                    }

                    // In case of error, forward it to the Promise.
                    if (future.hasError() && !promise.getFuture().isDone()) {
                        promise.setError(future.getErrorMessage());
                    }
                });
            });

    // Return the Future associated to the Promise.
    return promise.getFuture().thenCompose(future -> {
        // Stop the LocalizeAndMap.
        localizeAndMapFuture.cancel(true);
        return future;
    });
}

private void startMapExtensionStep(ExplorationMap initialExplorationMap, QiContext qiContext) {
    // Disable "extend map" button.
    extendMapButton.setEnabled(false);
    // Start the map extension and notify each time the map is updated.
    extendMap(initialExplorationMap, qiContext, updatedMap -> {
        // Convert the map to a bitmap.
        Bitmap updatedBitmap = mapToBitmap(updatedMap);
        // Display the bitmap.
        runOnUiThread(() -> {
            if (getLifecycle().getCurrentState().isAtLeast(Lifecycle.State.RESUMED)) {
                displayMap(updatedBitmap);
            }
        });
    }).thenConsume(future -> {
        // If the operation is not a success, re-enable "extend map" button.
        if (!future.isSuccess()) {
            runOnUiThread(() -> {
                if (getLifecycle().getCurrentState().isAtLeast(Lifecycle.State.RESUMED)) {
                    extendMapButton.setEnabled(true);
                }
            });
        }
    });
}

extendMapButton.setOnClickListener(ignored -> {
    // Check that an initial map is available.
    if (initialExplorationMap == null) return;
    // Check that the Activity owns the focus.
    if (qiContext == null) return;
    // Start the map extension step.
    startMapExtensionStep(initialExplorationMap, qiContext);
});