Challenge 10: Improve the subgoal visitation check

Preparation

Go to your repository, return in challenge_9 branch and create a new one:

git checkout challenge_9
cd ~/catkin_ws/src/autonomous_systems_architectures/
git checkout -b challenge_10

Task

The task is to fill the function checkTarget located here. Here the next subtarget is checked for proximity. If the robot is too close to the subtarget it is considered approached and the next subtarget is assigned as next. However there are cases where the robot misses one subtarget due to obstacle detection. Enhance this functionality by checking all the subgoals (and the final goal) for proximity and assign the proper next subgoal.

Points

5 out of extra 50