Merge "Add PathFinder algorithm using SPFA" into devel/master