Nav part in substatemachine
There is a concurrent state but this does not do anything useful yet
Preemption works
Correct bug in child_term_cb
Merge branch 'refactor/sergio+ed' of https://github.com/tue-robotics/robot_smach_states into refactor/sergio+ed