Fix for null pointer exception

Under certain conditions or if all paths are exausted sortedOpenNodes.poll(); will return null.