How to use topological navigation

Topological Navigation

Topological navigation is a tool that allows the user to define locations in the metric map and how to move between them by specifying the navigation actions the robot can make.

Topological Map

A topological map is a list of nodes (locations) taht can be defined in a YAML format and looks like this:

- node:                                    # A node
    name: WayPoint1                        # Node Name
    pointset: MAP_NAME                     # Map name
    edges:                                 # List of connections from this node
    - action: NAOqiPlanner/Goal            # Action connecting this node to the next
      edge_id: WayPoint1_WayPoint2         # Name of the connection
      node: WayPoint2                      # Name of the node that is connected by this edge 
    pose:                                  # Position of the node
      orientation:
        w: 0.776898920536
        x: 4.04172055823e-08
        y: 4.60677940239e-09
        z: -0.629625380039
      position:
        x: -1.77065169811
        y: -3.21753334999
        z: 0.00247192382812
    verts:                                  # Vertices of the influence area
    - x: 0.689999997616                     # (area around the node considered to be the same location)
      y: 0.287000000477
    - x: 0.287000000477
      y: 0.689999997616
    - x: -0.287000000477
      y: 0.689999997616
    - x: -0.689999997616
      y: 0.287000000477
    - x: -0.689999997616
      y: -0.287000000477
    - x: -0.287000000477
      y: -0.689999997616
    - x: 0.287000000477
      y: -0.689999997616
    - x: 0.689999997616
      y: -0.287000000477
- node:                                      # Another node (same fields)
    edges:
    - action: NAOqiPlanner/Goal
      edge_id: WayPoint2_WayPoint1
      node: WayPoint1
    name: WayPoint2
    pointset: MAP_NAME
    pose:
      orientation:
        w: 0.384249478579
        x: 1.91584863529e-08
        y: -1.46522545208e-08
        z: -0.923229336739
      position:
        x: 0.58447510004
        y: -1.29200899601
        z: 0.00247192382812
    verts:
    - x: 0.689999997616
      y: 0.287000000477
    - x: 0.287000000477
      y: 0.689999997616
    - x: -0.287000000477
      y: 0.689999997616
    - x: -0.689999997616
      y: 0.287000000477
    - x: -0.689999997616
      y: -0.287000000477
    - x: -0.287000000477
      y: -0.689999997616
    - x: 0.287000000477
      y: -0.689999997616
    - x: 0.689999997616
      y: -0.287000000477

Create a topological map

To create a topological map you can use the provided tool, you will only need a metric map of the environment (ros format). To use it you can use the following command:

<path_to>/map_editor.py --empty true --map metric-map.yaml --outmap topological-map.tpg

This will open a window with the metric map and there you can use the following commands:

Press (a) to add node
Press (d) to remove node
Press (e) to add edge between to nodes (click origin first and then destination)
Press (h) for this Menu
Press (m) to move a node
Press (w) to write map to outmap

To edit a previously created topological map you can do:

<path_to>/map_editor.py --tmap topological-map.tpg --map metric-map.yaml --outmap topological-map.tpg

Use topological navigation

To use topological navigation you will need the planner and localiser provided in this package to be running and then you can run it using your topological map using the following command

<path_to>/topological_navigation.py --tmap out.tpg

To test it you can raise a TopologicalNav/Goal event with the name of your destination. Or use the provided nav_client:

<path_to>/nav_client.py NameOfYourDestination