Found the bug first bug with replanning, which caused the search to throw an exception. Now it atleast starts to find a new solution, but still when constructing new path it keeps looping.