Work on fail_policy

[Jailander]

We need to decide what our fail policy will look like @gpdas has had some previous work on it and he suggests that additionally from the 3 options presented in #35 (Fail, replan, retry), we should also have a wait option (i.e. wait_(time in seconds)_(max times to wait).

This also need to be implemented on new version of topomap. Maybe we should consider implementing fail policies as a plugin so people can have custom policies implemented. Also recovery behaviours should be considered here and not in the lower level action.

[adambinch]

Done