Updated breakout function navigate to observe
Added navigate to grasp, needs some work ;)
Merge branch 'refactor/sergio+ed' of https://github.com/tue-robotics/robot_smach_states into refactor/sergio+ed