SoftBank Robotics documentation What's new in NAOqi 2.5?

Exploration API

Overview | API Navigation | API Exploration

Namespace : AL

#include <alproxies/alnavigationproxy.h>

Experimental Methods

qi::Future<int> ALNavigationProxy::explore(float radius)

Warning

This is still a work in progress, and API may change in the future.

Makes Pepper explore autonomously its surrounding environment in the limit of the radius passed as parameter.

Parameters:
  • radius – distance max to explore in meters.
Returns:

a future holding the Error code corresponding to the final status of the exploration.
void ALNavigationProxy::stopExploration()

Warning

This is still a work in progress, and API may change in the future.

Stops any ongoing exploration or localization. No-op if nothing is running.

std::string ALNavigationProxy::saveExploration()

Warning

This is still a work in progress, and API may change in the future.

Saves the current exploration data on disk for a later use.

Returns:a string indicating the path of the created .explo file.
ALValue ALNavigationProxy::getMetricalMap()

Warning

This is still a work in progress, and API may change in the future.

Returns the map based on the currently loaded exploration. The map is formatted as follow: [mpp, width, height, [originOffsetX, originOffsetY], [pxlVal, ...]]

where:

  • mpp is the resolution of the map in meters per pixel,
  • width and height are the size of the image in pixels,
  • originOffset is the metrical offset of the pixel (0, 0) of the map,
  • and [pxlVal, ...] is the buffer of pixel floating point values between 0 (occupied space) and 100 (free space).

Throws if no exploration is loaded or if the localization is not running.

Returns:an ALValue containing the map formatted as explained.
qi::Future<int> ALNavigationProxy::navigateToInMap(const std::vector<float>& target)

Warning

This is still a work in progress, and API may change in the future.

Makes Pepper navigate to a desired target by using the explored map.

Throws if no exploration is loaded or if the localization is not running.

Note: for now, Pepper does NOT use final theta target, but only XY position. For final orientation, you can use a ALMotion.moveTo of the desired angle.

Parameters:
  • target – [x, y, theta] of the target in explorated map frame.
Returns:

a future holding an error code representing the status of the execution.
ALValue ALNavigationProxy::getRobotPositionInMap()

Warning

This is still a work in progress, and API may change in the future.

Retrieves the localized position of the robot in the current explored map frame.

Throws if no exploration is loaded or if the localization is not running.

Returns:an ALValue containing a Localized Pose.
qi::Future<bool> ALNavigationProxy::loadExploration(std::string path)

Warning

This is still a work in progress, and API may change in the future.

Loads the .explo from the location on the disk.

Returns:a future holding the success of the exploration loading.
qi::Future<ALValue> ALNavigationProxy::relocalizeInMap(const std::vector<float>& estimation)

Warning

This is still a work in progress, and API may change in the future.

Requests the localizer to relocalize around an estimated pose.

Throws if no exploration is loaded.

Parameters:
  • estimation – [x, y, theta] of the estimated pose of the robot in map frame.
Returns:

a Future holding the resulting localized pose formatted as follow.
void ALNavigationProxy::startLocalization()

Warning

This is still a work in progress, and API may change in the future.

Starts the exploration loop that computes the robot localization.

Throws if no exploration is loaded.

void ALNavigationProxy::stopLocalization()

Warning

This is still a work in progress, and API may change in the future.

Stops the exploration loop that computes the robot localization. No-op if localization loop is not running.

Custom types

Localized Pose

Localized pose is formatted as following:

[Pose, Uncertainty]

where:

  • Pose is the position [X, Y, theta] computed by the localizer
  • Uncertainty is the ellipse with parameters [radiusX, radiusY, orientation] defining the uncertainty ellipse of the localized pose

Error code

Value Meaning
0 OK
1 KO
2 Canceled
3 TargetReached
4 TargetNotReached
5 TargetOutOfMap

Code samples

Python script for exploring

The following script makes the robot explore a 2 m radius zone, and if everything went OK, saves the exploration on disk, comes back to his starting point, and displays the result map.

explore.py

#! /usr/bin/env python
# -*- encoding: UTF-8 -*-

"""Example: Use explore method."""

import qi
import argparse
import sys
import numpy
import Image


def main(session):
    """
    This example uses the explore method.
    """
    # Get the services ALNavigation and ALMotion.
    navigation_service = session.service("ALNavigation")
    motion_service = session.service("ALMotion")

    # Wake up robot
    motion_service.wakeUp()

    # Explore the environement, in a radius of 2 m.
    radius = 2.0
    error_code = navigation_service.explore(radius)
    if error_code != 0:
        print "Exploration failed."
        return
    # Saves the exploration on disk
    path = navigation_service.saveExploration()
    print "Exploration saved at path: \"" + path + "\""
    # Start localization to navigate in map
    navigation_service.startLocalization()
    # Come back to initial position
    navigation_service.navigateToInMap([0., 0., 0.])
    # Stop localization
    navigation_service.stopLocalization()
    # Retrieve and display the map built by the robot
    result_map = navigation_service.getMetricalMap()
    map_width = result_map[1]
    map_height = result_map[2]
    img = numpy.array(result_map[4]).reshape(map_width, map_height)
    img = (100 - img) * 2.55 # from 0..100 to 255..0
    img = numpy.array(img, numpy.uint8)
    Image.frombuffer('L',  (map_width, map_height), img, 'raw', 'L', 0, 1).show()

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", type=str, default="127.0.0.1",
                        help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
    parser.add_argument("--port", type=int, default=9559,
                        help="Naoqi port number")

    args = parser.parse_args()
    session = qi.Session()
    try:
        session.connect("tcp://" + args.ip + ":" + str(args.port))
    except RuntimeError:
        print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
               "Please check your script arguments. Run with -h option for help.")
        sys.exit(1)
    main(session)

Python script for localizing

The following script loads a map given in argument, relocalizes Pepper inside this map, and navigates to a position in the map.

localize.py

#! /usr/bin/env python
# -*- encoding: UTF-8 -*-

"""Example: Use localization methods"""

import qi
import argparse
import sys


def main(session, exploration_file):
    """
    This example uses localization methods.
    """
    # Get the services ALNavigation and ALMotion.
    navigation_service = session.service("ALNavigation")
    motion_service = session.service("ALMotion")

    # Wake up robot
    motion_service.wakeUp()

    # Load a previously saved exploration
    navigation_service.loadExploration(exploration_file)

    # Relocalize the robot and start the localization process.
    guess = [0., 0.] # assuming the robot is not far from the place where
                     # he started the loaded exploration.
    navigation_service.relocalizeInMap(guess)
    navigation_service.startLocalization()

    # Navigate to another place in the map
    navigation_service.navigateToInMap([2., 0., 0.])

    # Check where the robot arrived
    print "I reached: " + str(navigation_service.getRobotPositionInMap()[0])

    # Stop localization
    navigation_service.stopLocalization()

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", type=str, default="127.0.0.1",
                        help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
    parser.add_argument("--port", type=int, default=9559,
                        help="Naoqi port number")
    parser.add_argument("--explo", type=str, help="Path to .explo file.")

    args = parser.parse_args()
    session = qi.Session()
    try:
        session.connect("tcp://" + args.ip + ":" + str(args.port))
    except RuntimeError:
        print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
               "Please check your script arguments. Run with -h option for help.")
        sys.exit(1)
    main(session, args.explo)