Speed up your algorithms (python)

Run Forrest! Run !

Solution

Here is a possible solution. You can note that we have queried the indexes of the properties "isAlong" and "hasAlong" only once at the initialisation to avoid to recompute static information. In a more usual way using classes, you could create a map with all the "static" indexes.

#!/usr/bin/env python

import rospy

from ontologenius import OntologyManipulatorIndex

_onto = None
isAlong_index = 0
hasAlong_index = 0

def displayRoute(route):
print('Route:')
route_id = _onto.conversion.individualsIndexes2Ids(route)
for point in route_id:
print('- ' + point)

def findRoutes(path_from, path_to, selector):
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_index, route[size])

for intersection in intersections:
paths = _onto.individuals.getOn(intersection, isAlong_index, selector)

# 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

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

paths_from = _onto.individuals.getOn(place_from, isAlong_index, selector)
paths_to = _onto.individuals.getFrom(hasAlong_index, place_to, selector)

for path_from in paths_from:
for path_to in paths_to:
routes = findRoutes(path_from, path_to, selector)
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

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

_onto = OntologyManipulatorIndex()
_onto.close()

isAlong_index = _onto.conversion.objectPropertiesId2Index('isAlong')
hasAlong_index = _onto.conversion.objectPropertiesId2Index('hasAlong')

route = find(_onto.conversion.individualsId2Index('pub'),
_onto.conversion.individualsId2Index('house'),
_onto.conversion.classesId2Index('>path'))
displayRoute(route)

route = find(_onto.conversion.individualsId2Index('house'),
_onto.conversion.individualsId2Index('car'),
_onto.conversion.classesId2Index('path'))
displayRoute(route)

route = find(_onto.conversion.individualsId2Index('car'),
_onto.conversion.individualsId2Index('house'),
_onto.conversion.classesId2Index('road'))
displayRoute(route)

if __name__ == '__main__':
main():