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 fun startMapping(qiContext: QiContext) {
    // Here we will map Pepper's surroundings.
}

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

Then call it in the onRobotFocusGained method:

override fun onRobotFocusGained(qiContext: QiContext) {
    startMapping(qiContext)
}

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

Add a LocalizeAndMap field in your MainActivity:

// Store the LocalizeAndMap action.
private var localizeAndMap: LocalizeAndMap? = null

// 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()

// 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 ->
     if (status == LOCALIZED) {
            Log.i(TAG, "Robot has mapped his environment.")
    }
}

// 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.
localizeAndMap?.removeAllOnStatusChangedListeners()

// 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 var localizationAndMapping: Future<Void>? = null

// 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()

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 var explorationMap: ExplorationMap? = null

// 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()

// 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()

// 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.error)
    } else if (future.isCancelled) {
        // The LocalizeAndMap action has been 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()) {
        // The LocalizeAndMap action has been cancelled.
    }
});

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

private fun 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 ->
        if (status == LOCALIZED) {
                // Dump the ExplorationMap.
                explorationMap = localizeAndMap.dumpMap()

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

                // Cancel the LocalizeAndMap action.
                localizationAndMapping?.requestCancellation()
        }
    }

    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.error)
        } else if (future.isCancelled) {
            // The LocalizeAndMap action has been cancelled.
        }
    }
}

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 fun startLocalizing(qiContext: QiContext) {
    // Here we will make Pepper localize himself in the map.
}

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.error)
    } else if (future.isCancelled) {
        startLocalizing(qiContext)
    }
}

// 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 var localize: Localize? = null

// Store the Localize action.
private Localize localize;

The startLocalizing implementation is very similar to the startMapping one:

private fun startLocalizing(qiContext: QiContext) {
    // Create a Localize action.
    localize: 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 ->
        if (status == LOCALIZED) {
            Log.i(TAG, "Robot is localized.")
        }
    }

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

    // Execute the Localize action asynchronously.
    val 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.error)
        }
    }
}

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.
localize?.removeAllOnStatusChangedListeners()

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

Let’s try it¶

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.

../../../_images/detect_humans_with_loc.png

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

Note

To go further and learn how to extend a map, see Extend a map, display it.