Do not abort current behaviour after emergency is cleared

[2m]

When emergency is cleared, mower should continue its last behaviour. This will make the mower to continue mowing when the emergency status is cleared manually.

Fixes #52

[2m]

Tested it out and works as expected.

[ClemensElflein]

I'm not sure if this fix is correct, as far as I know, when the emergency occurs the mowing behavior will still try to continue mowing (which of course fails, because the mower is in emergency), then skip a bit of the path and then continue mowing, so when the emergency is cleared after some time, mowing will resume at some other future position.

I think in order to fix this, we need to set the mowing behavior to "paused" in the event of an emergency and resume it, when the emergency is cleared.

Maybe I'm wrong on this, though.

[2m]

... when the emergency occurs the mowing behavior will still try to continue mowing (which of course fails, because the mower is in emergency), then skip a bit of the path and then continue mowing

Could it be that without this fix, the mower keeps on trying to skip to the next path until eventually it goes to DOCKING state? This is the behaviour that I remember when I was trying to fix it: when I clear the emergency, the mower would go to the DOCKING state instead of going to the next path to mow.

[ClemensElflein]

With the abort() it should abort the current behavior which will put it into docking, but since the emergency persists, docking should be also aborted which should put it into idle.

Without the abort() (that was the earlier behavior) it would try to continue, but fail and eventually try to dock. The docking behavior was not aborted though and that's why when releasing the emergency, it would be in docking mode.

The correct implementation should pause the mow instead of abort (similar to GPS loss) and resume the mow once emergency is resolved.

[olliewalsh]

Also kind-of related to this: pause/continue needs to save/restore the mowEnabled state instead of always enabling mowing on continue

[olliewalsh]

Testing this with https://github.com/olliewalsh/open_mower_ros/commit/a4b3a02b404a62e70e5290d8c9fefdb1accddd74 ... I don't think it's quite right yet though. Think it needs to track why pause was requested and only continue when all have requested continued

[olliewalsh]

Currently using with https://github.com/ClemensElflein/open_mower_ros/commit/318a28d26cc73aa0816e65335ee2882c48c1e2c6 but not had time to really test it

[olliewalsh]

@2m hope you don't mind, I created a new PR to combine both patches and rebase on latest main

[2m]

Thank you very much! I am not able to test right now, so could not continue on this PR further.