Controller FSM modifications

[tarapt]

Modify the FSM to ensure that a robot which has to wait at intersection due to some obstacle, moves to a non-intersection cell as soon as possible.

Also, modify the FSM to handle traffic lights and replanning.