Automatically map the shortest distance between points of interest

[gabrielsr]

In a multi-robot scenario, before delegating a task for a robot we frequently need to consider the distance between the actual position of a candidate robot and the position where the action should be executed.

Although this distance can be calculated when necessary, this on-the-fly calculation may not be very efficient if we need to calculate it frequently.

While implementing a planner, a nice to have feature would be a service accessible at the beginning of the simulation that could be used to get the distance between two points of interest (poi).
Ex:
distance = map_resource.shortest_path_distance(poi1, poi2)

The idea is to map the shortest routes between two points before starting the simulation. The points of interest should be created as part of the map.

The routes could be part of the map or automatically discovered before initing the 'map_resource'.