Detect humans with localization

Goal

In this tutorial, we will use Pepper’s localization ability to enhance the detection and the tracking of humans.

Prerequisites

Before stepping in this tutorial, you should be familiar with the robot focus and robot lifecycle notions.

For further details, see: Mastering Focus & Robot lifecycle.

Let’s start a new project

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

For further details, see: Creating a robot application.

Mapping Pepper’s surroundings

First, we need to create a map of Pepper’s surroundings. To achieve this, we will use the LocalizeAndMap action.

Add the startMapping method to your MainActivity class:

private void startMapping(QiContext qiContext) {
    // Here we will map Pepper's surroundings.
}

Then call it in the onRobotFocusGained method:

@Override
public void onRobotFocusGained(QiContext qiContext) {
    startMapping(qiContext);
}

Add a LocalizeAndMap field in your MainActivity:

// Store the LocalizeAndMap action.
private LocalizeAndMap localizeAndMap;

Create it with a LocalizeAndMapBuilder in the startMapping method:

// Create a LocalizeAndMap action.
localizeAndMap = LocalizeAndMapBuilder.with(qiContext).build();

The LocalizeAndMap interface has an addOnStatusChangedListener method that allows us to be notified when the localization status of the LocalizeAndMap action changes. We will use it to know when Pepper has mapped his environment.

Add the following code in the startMapping method:

// Add an on status changed listener on the LocalizeAndMap action to know when the robot has mapped his environment.
localizeAndMap.addOnStatusChangedListener(status -> {
    switch (status) {
        case LOCALIZED:
            Log.i(TAG, "Robot has mapped his environment.");
            break;
    }
});

Do not forget to remove this listener on LocalizeAndMap in the onRobotFocusLost method:

// Remove on status changed listeners from the LocalizeAndMap action.
if (localizeAndMap != null) {
    localizeAndMap.removeAllOnStatusChangedListeners();
}

We can now run the LocalizeAndMap action. Add a Future<Void> field in your MainActivity:

// Store the LocalizeAndMap execution.
private Future<Void> localizationAndMapping;

And run the LocalizeAndMap action asynchronously in the startMapping method:

Log.i(TAG, "Mapping...");

// Execute the LocalizeAndMap action asynchronously.
localizationAndMapping = localizeAndMap.async().run();

We will now retrieve the map of Pepper’s surroundings. Add an ExplorationMap field in your MainActivity:

// Store the map.
private ExplorationMap explorationMap;

In the addOnStatusChangedListener method, in the LOCALIZED case, add the following code to get the map:

// Dump the ExplorationMap.
explorationMap = localizeAndMap.dumpMap();

Now that we have the map, we don’t need the LocalizeAndMap action to run anymore, so we can add the following code to stop its execution, just after retrieving the map:

// Cancel the LocalizeAndMap action.
localizationAndMapping.requestCancellation();

In the next step of this tutorial, we will need to know when the LocalizeAndMap action actually stops, so add the following code in the startMapping method:

// Add a lambda to the action execution.
localizationAndMapping.thenConsume(future -> {
    if (future.hasError()) {
        Log.e(TAG, "LocalizeAndMap action finished with error.", future.getError());
    } else if (future.isCancelled()) {
        // The LocalizeAndMap action has been cancelled.
    }
});

To recap, here is what the whole startMapping method should look like:

private void startMapping(QiContext qiContext) {
    // Create a LocalizeAndMap action.
    localizeAndMap = LocalizeAndMapBuilder.with(qiContext).build();

    // Add an on status changed listener on the LocalizeAndMap action to know when the robot has mapped his environment.
    localizeAndMap.addOnStatusChangedListener(status -> {
        switch (status) {
            case LOCALIZED:
                // Dump the ExplorationMap.
                explorationMap = localizeAndMap.dumpMap();

                Log.i(TAG, "Robot has mapped his environment.");

                // Cancel the LocalizeAndMap action.
                localizationAndMapping.requestCancellation();
                break;
        }
    });

    Log.i(TAG, "Mapping...");

    // Execute the LocalizeAndMap action asynchronously.
    localizationAndMapping = localizeAndMap.async().run();

    // Add a lambda to the action execution.
    localizationAndMapping.thenConsume(future -> {
        if (future.hasError()) {
            Log.e(TAG, "LocalizeAndMap action finished with error.", future.getError());
        } else if (future.isCancelled()) {
            // The LocalizeAndMap action has been cancelled.
        }
    });
}

Localizing Pepper in a map

In order to enhance Pepper’s ability to detect and track humans, we need him to be localized in a map.

Add the startLocalizing method to your MainActivity class:

private void startLocalizing(QiContext qiContext) {
    // Here we will make Pepper localize himself in the map.
}

Then call it in when localizationAndMapping is cancelled:

// Add a lambda to the action execution.
localizationAndMapping.thenConsume(future -> {
    if (future.hasError()) {
        Log.e(TAG, "LocalizeAndMap action finished with error.", future.getError());
    } else if (future.isCancelled()) {
        startLocalizing(qiContext);
    }
});

Add a Localize field in your MainActivity:

// Store the Localize action.
private Localize localize;

The startLocalizing implementation is very similar to the startMapping one:

private void startLocalizing(QiContext qiContext) {
    // Create a Localize action.
    localize = LocalizeBuilder.with(qiContext)
            .withMap(explorationMap)
            .build();

    // Add an on status changed listener on the Localize action to know when the robot is localized in the map.
    localize.addOnStatusChangedListener(status -> {
        switch (status) {
            case LOCALIZED:
                Log.i(TAG, "Robot is localized.");
                break;
        }
    });

    Log.i(TAG, "Localizing...");

    // Execute the Localize action asynchronously.
    Future<Void> localization = localize.async().run();

    // Add a lambda to the action execution.
    localization.thenConsume(future -> {
        if (future.hasError()) {
            Log.e(TAG, "Localize action finished with error.", future.getError());
        }
    });
}

In this implementation, we:

  • Use a LocalizeAndMapBuilder with the stored ExplorationMap to create the Localize action.
  • Use the addOnStatusChangedListener to know when the robot is localized in the map.
  • Run the Localize action asynchronously.
  • Log an error if the action execution fails.

Do not forget to remove the listener on Localize in the onRobotFocusLost method:

// Remove on status changed listeners from the Localize action.
if (localize != null) {
    localize.removeAllOnStatusChangedListeners();
}

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 “Detect humans with localization”.

Wait for pepper to map his environment.

Wait for Pepper to localize himself in the map.

When Pepper is localized in his environment, try to come from behind him and he will detect you.

https://developer.softbankrobotics.com/sites/default/files/repository/60_rst_pepper/public/source/_build/html/_images/detect_humans_with_loc.png

You are now able to use localization to detect humans with Pepper!