Extend a map, display it

Goal

In this tutorial, we will see how to display the graphical representation of an ExplorationMap, and how to extend a previously created map.

Prerequisites

Before stepping in this tutorial, you should:

Let’s start a new project

  • Start a new project, let’s call it ExtendMapPepper.
  • Robotify it and make sure it implements the QiSDK & the Robot Life Cycle.

For further details, see: Creating a robot application.

UI setup

Let’s start by adding the UI for the application. Put the following code in activity_main.xml:

<?xml version="1.0" encoding="utf-8"?>
<androidx.constraintlayout.widget.ConstraintLayout xmlns:android="http://schemas.android.com/apk/res/android"
    xmlns:app="http://schemas.android.com/apk/res-auto"
    xmlns:tools="http://schemas.android.com/tools"
    android:layout_width="match_parent"
    android:layout_height="match_parent">

    <ImageView
        android:id="@+id/mapImageView"
        android:layout_width="0dp"
        android:layout_height="0dp"
        android:adjustViewBounds="true"
        android:layout_marginTop="16dp"
        android:layout_marginBottom="16dp"
        app:layout_constraintBottom_toTopOf="@+id/startMappingButton"
        app:layout_constraintEnd_toEndOf="parent"
        app:layout_constraintStart_toStartOf="parent"
        app:layout_constraintTop_toTopOf="parent"/>

    <Button
        android:id="@+id/startMappingButton"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
        android:text="Start mapping"
        android:layout_marginBottom="16dp"
        app:layout_constraintBottom_toBottomOf="parent"
        app:layout_constraintEnd_toStartOf="@+id/extendMapButton"
        app:layout_constraintStart_toStartOf="parent"/>

    <Button
        android:id="@+id/extendMapButton"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
        android:text="Extend map"
        app:layout_constraintBottom_toBottomOf="@+id/startMappingButton"
        app:layout_constraintEnd_toEndOf="parent"
        app:layout_constraintStart_toEndOf="@+id/startMappingButton"
        app:layout_constraintTop_toTopOf="@+id/startMappingButton"/>

</androidx.constraintlayout.widget.ConstraintLayout>

It consists of:

  • An ImageView to display the map graphical representation.
  • A Button to start the mapping.
  • A Button to extend the map.

Initial mapping

Add a QiContext field in the MainActivity:

// The QiContext provided by the QiSDK.
private var qiContext: QiContext? = null

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

Then to store the QiContext, put the following code in the onRobotFocusGained method:

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

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

And, to remove it, add the following code in the onRobotFocusLost method:

// Remove the QiContext.
this.qiContext = null

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

Add a mapSurroundings method to map Pepper’s surroundings and retrieve the corresponding map. For this, we’ll use the LocalizeAndMap action:

private fun mapSurroundings(qiContext: QiContext): Future<ExplorationMap> {
    // Create a Promise to set the operation state later.
    val promise = Promise<ExplorationMap>().apply {
        // If something tries to cancel the associated Future, do cancel it.
        setOnCancel {
            if (!it.future.isDone) {
                setCancelled()
            }
        }
    }

    // Create a LocalizeAndMap, run it, and keep the Future.
    val 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.
                        val explorationMap = localizeAndMap.dumpMap()
                        // Set the Promise state in success, with the ExplorationMap.
                        if (!promise.future.isDone) {
                            promise.setValue(explorationMap)
                        }
                    }
                }

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

    // Return the Future associated to the Promise.
    return promise.future.thenCompose {
        // Stop the LocalizeAndMap.
        localizeAndMapFuture.cancel(true)
        return@thenCompose it
    }
}

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;
    });
}

Store an ExplorationMap in the MainActivity, it will be the initial map:

// The initial ExplorationMap.
private var initialExplorationMap: ExplorationMap? = null

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

Add a mapToBitmap method to convert an ExplorationMap to a Bitmap. We’ll use the ExplorationMap.topGraphicalRepresentation method:

private fun mapToBitmap(explorationMap: ExplorationMap): Bitmap {
    // Get the ByteBuffer containing the map graphical representation.
    val byteBuffer = explorationMap.topGraphicalRepresentation.image.data.apply { rewind() }
    // Get the buffer size.
    val size = byteBuffer.remaining()
    // Transform the buffer to a ByteArray.
    val byteArray = ByteArray(size).also { byteBuffer.get(it) }
    // Transform the ByteArray to a Bitmap.
    return BitmapFactory.decodeByteArray(byteArray, 0, size)
}

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);
}

Add a displayMap method to display the map in an ImageView:

private fun displayMap(bitmap: Bitmap) {
    // Set the ImageView bitmap.
    mapImageView.setImageBitmap(bitmap)
}

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

Note

In Java, do not forget to find mapImageView in the onCreate method and store it in the MainActivity.

Add a startMappingStep method to start the mapping step. It will use the previously created methods:

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

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);
                }
            });
        }
    });
}

Note

In Java, do not forget to find startMappingButton and extendMapButton in the onCreate method and store them in the MainActivity.

In the onCreate method, call startMappingStep when the startMappingButton is clicked:

// Set the button onClick listener.
startMappingButton.setOnClickListener {
    // Check that the Activity owns the focus.
    val qiContext = qiContext ?: return@setOnClickListener
    // Start the mapping step.
    startMappingStep(qiContext)
}

// 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 the onResume method to reset the UI state when the MainActivity appears:

override fun onResume() {
    super.onResume()
    // Reset UI and variables state.
    startMappingButton.isEnabled = false
    extendMapButton.isEnabled = false
    initialExplorationMap = null
    mapImageView.setImageBitmap(null)
}

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

And put the following code in the onRobotFocusGained method:

// Enable "start mapping" button.
runOnUiThread {
    startMappingButton.isEnabled = true
}

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

Note

It is possible to save the ExplorationMap in a File to reuse it later. You can find an example in Return To Map Frame.

Map extension

Add a publishExplorationMap method. It will use a LocalizeAndMap action and a callback to get the current ExplorationMap every 2 seconds:

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

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);
    });
}

Add an extendMap method to extend a map and be notified when a new map is available. We’ll use the LocalizeAndMapBuilder.withMap method to extend the initial map:

private fun extendMap(explorationMap: ExplorationMap, qiContext: QiContext, updatedMapCallback: (ExplorationMap) -> Unit): Future<Void> {
    // Create a Promise to set the operation state later.
    val promise = Promise<Void>().apply {
        // If something tries to cancel the associated Future, do cancel it.
        setOnCancel {
            if (!it.future.isDone) {
                setCancelled()
            }
        }
    }

    // Create a LocalizeAndMap with the initial map, run it, and keep the Future.
    val localizeAndMapFuture = LocalizeAndMapBuilder.with(qiContext)
            .withMap(explorationMap)
            .buildAsync()
            .andThenCompose { localizeAndMap ->
                // Create a Future for map notification.
                var publishExplorationMapFuture: Future<Void>? = null

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

                // Run the LocalizeAndMap.
                localizeAndMap.async().run().thenConsume {
                    // Remove the OnStatusChangedListener.
                    localizeAndMap.removeAllOnStatusChangedListeners()
                    // Stop the map notification process.
                    publishExplorationMapFuture?.cancel(true)
                    // In case of error, forward it to the Promise.
                    if (it.hasError() && !promise.future.isDone) {
                        promise.setError(it.errorMessage)
                    }
                }
            }

    // Return the Future associated to the Promise.
    return promise.future.thenCompose {
        // Stop the LocalizeAndMap.
        localizeAndMapFuture.cancel(true)
        return@thenCompose it
    }
}

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;
    });
}

Add a startMapExtensionStep method to start the map extension step. It will use the previously created methods:

private fun startMapExtensionStep(initialExplorationMap: ExplorationMap, qiContext: QiContext) {
    // Disable "extend map" button.
    extendMapButton.isEnabled = false
    // Start the map extension and notify each time the map is updated.
    extendMap(initialExplorationMap, qiContext) { updatedMap ->
        // Convert the map to a bitmap.
        val updatedBitmap = mapToBitmap(updatedMap)
        // Display the bitmap.
        runOnUiThread {
            if (lifecycle.currentState.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 (lifecycle.currentState.isAtLeast(Lifecycle.State.RESUMED)) {
                    extendMapButton.isEnabled = true
                }
            }
        }
    }
}

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);
                }
            });
        }
    });
}

In the onCreate method, call startMapExtensionStep when the extendMapButton is clicked:

extendMapButton.setOnClickListener {
    // Check that an initial map is available.
    val initialExplorationMap = initialExplorationMap ?: return@setOnClickListener
    // Check that the Activity owns the focus.
    val qiContext = qiContext ?: return@setOnClickListener
    // Start the map extension step.
    startMapExtensionStep(initialExplorationMap, qiContext)
}

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);
});

Let’s try it

github_icon The sources for this tutorial are available on GitHub.

Step Action

Install and run the application.

For further details, see: Running an application.

Choose “Extend a map”.
Close the robot’s charging flap.
Click on “Start mapping”.
Wait for the robot to complete its 360° turn. The map should appear on the tablet.
Click on “Extend map”.
Wait for the map representation to update.
Open the robot’s charging flap.
Give the robot a tour by gently and securely pushing it, one hand on its lower back the other on its shoulder.

Result: see the map updating as the robot moves.

/sites/default/files/repository/64_rst_pepper/_build/html/_images/extend_map.png

You are now able to extend a map and display it on Pepper’s tablet!