Advanced knowledge exploration (python)

How to go home ?

Find our way

Before starting, here is the body of our program with a function that will allow us to display routes. Routes will be represented by a list of strings.

#!/usr/bin/env python

import rospy

from ontologenius import OntologyManipulator

_onto = None

def displayRoute(route):
print('Route:')
for point in route:
print('- ' + point)

def main():
global _onto
rospy.init_node('find_our_way')

_onto = OntologyManipulator()
_onto.close()

#
# We will find the route here
#

if __name__ == '__main__':
main()

We can notice the presence of the ontology manipulator object.

def findRoutes(path_from, path_to):
global _onto
routes = []
routes.insert(0, [path_from])

found = False
if path_from == path_to:
found = True

while not found:
tmp_routes = []
for route in routes:
size = len(route) - 1
intersections = _onto.individuals.getFrom('isAlong', route[size])

for intersection in intersections:
paths = _onto.individuals.getOn(intersection, 'isAlong')

# erase paths already present in the current route
paths = [path for path in paths if not path in route]

for path in paths:
tmp = list(route)
tmp.insert(len(tmp), intersection)
tmp.insert(len(tmp), path)
tmp_routes.insert(len(tmp_routes), tmp)
if path == path_to:
found = True

routes = list(tmp_routes)

return routes

Well, it's just a breadth-first search but it's still not obvious. We will analyze this together because two lines are particularly interesting.

def findRoutes(path_from, path_to):

Let's start with the beginning. This function was used to search for routes linking "path_from" to "path_to".

routes = []

"routes" represents a list of routes since a route is itself a list of strings.

routes.insert(0, [path_from])

We initialize our list of routes with a route only composed of the start path.

tmp_routes = []
for route in routes:

Since we use a breadth-first search, we browse the set of current routes at each iteration and we rank the routes found at the current iteration in the variable tmp_routes.

intersections = _onto.individuals.getFrom('isAlong', route[size])

IMPORTANT :

We can represent the use of a property like this: individual_1 property individual_2
In the case of our paths, it comes down to this: intersection_i isAlong path_j

The getFrom function returns all the red individuals who apply the green property to the blue individual.
That means in our case that we are going to have all of the individuals along the path routes[route_i][size]. However, that means that we are going to have the intersections as well as the other elements, but that is not a big deal at the moment.

Notice how with a single line we can retrieve accurate information from our knowledge base.

for intersection in intersections:

We are now going through all of our intersections to see where they can lead us.

paths = _onto.individuals.getOn(intersection, 'isAlong')

IMPORTANT :

Let's take again the fact that: intersection_i isAlong path_j

The getOn function will make it possible to achieve the inverse of the getFrom function. We will retrieve the set of blue individuals that are connected to the red individual by the green property. In our case, we are going to have all the paths for which the intersection intersections[int_i] is along.

for path in paths:

For all paths not already present in the current route, we create a new route which is the current route plus the intersection leading to the new path as well as the new path.

routes = list(tmp_routes)

At the end of the current iteration, the new routes of work become the current ones.

Find a route from one place to another

The preceding function can find paths from one path to another except that we want a single path from one place to another. The following algorithm is doing this. We will analyze it quickly together.

def find(place_from, place_to):
global _onto
res_route = []
min_size = 100

paths_from = _onto.individuals.getOn(place_from, 'isAlong')
paths_to = _onto.individuals.getFrom('hasAlong', place_to)

for path_from in paths_from:
for path_to in paths_to:
routes = findRoutes(path_from, path_to)
for route in routes:
if route[len(route) - 1] == path_to:
if len(route) < min_size:
min_size = len(route)
res_route = route

return res_route
res_route = []
min_size = 100

"res_route" is going to be our final route and "min_size" will allow us to find the road with the least steps.

paths_from = _onto.individuals.getOn(place_from, 'isAlong')
paths_to = _onto.individuals.getFrom('hasAlong', place_to)

Now that you know the getOn and getFrom functions, you should understand what we are doing here. These two lines do exactly the same thing but in two different places (from and to). Indeed, hasAlong is the inverse property of isAlong or getOn is the inverse function of getFrom, so these two lines return to the same.

For each one of the places from and to we recover the paths of which they are along.

In the rest of the algorithm, we perform a search from the start path to the end path and only take the paths that actually lead to the end path and have a size smaller than the last shortest route found.

Update the main function

We can finally add the following lines in our main function to realize the path search for the three bob's itineraries.

route = find('pub', 'house')
displayRoute(route)

route = find('house', 'car')
displayRoute(route)

route = find('car', 'house')
displayRoute(route)

Launch the program. You should have the following result:

Route:
- road_2
- int_7
- pp_4
- int_12
- road_3
Route:
- road_3
- int_11
- pp_3
- int_10
- road_4
Route:
- road_4
- int_10
- pp_3
- int_11
- road_3