Inspect not robust

[MatthijsBurgh]

trafficstars

When a new nav goal is given during the navigate state of the inspect, the object is still fitted. Because the robot can be anywhere, the object can be fitted anywhere in the map.

[LoyVanBeek]

A possible cause:

1. state_machine sends a navgoal A
2. You give a navgoal B, which preempts A
3. State_machine still continues to next state, which is Inspect.

If this really is what happens, there ate 2 potential problems:

1. State_machine also continues to next state even when goal is preempted
2. preempted goal is not communicated to state_machine

[MatthijsBurgh]

Preemption of the executing a plan is not passed through to higher levels. https://github.com/tue-robotics/tue_robocup/blob/master/robot_smach_states/src/robot_smach_states/navigation/navigation.py#L260