Add checkForReachableTarget option in PathPlanning!

Signed-off-by: eldr4d <nikofinas@gmail.com>