Allow aborting Docking/Undocking

[jeremysalwen]

Pressing "Home" when undocking aborts it, and pressing "play" when docking aborts it.

Uninterruptable docking can be a real problem if your mower is stuck trying to return to the dock and you want to tele-operate it. This adds button controls, as well as actions which can be used from the GUI.

[jeremysalwen]

The GUI still needs updating, but the buttons on the mower work in my brief test. I just wanted to check that this would be the sort of thing you would merge. I find it very frustrating that a lot of the time it's impossible to get the mower into IDLE, since DOCKING and UNDOCKING are uninterruptible. Even MOWING you can't actually interrupt, you can only send it to DOCKING, which is no better. I have to restart the openmower container to get it back into idle.

This PR is pretty straightforward. The only thing that was up to my preference was I chose the Home button to stop undocking and the Play button to stop docking.

[prigorus]

This is something that is definitely needed. While this is right now a workaround, by using existing buttons, I think in the future it would make sense to add a stop button. This is something that other robot mowers have and I miss. If you are mowing and want to record an area - no chance to stop it unless you dock. Same if it is mowing and gets stuck, you have to wait until it tries to dock and fails and only then can you do other operations remotely.

[fallingcats]

I think the best UI for this would be that Play/Run and Home should abort and immediately do their thing, while pause should pause in either case and cancel when pressed again (in the UI visually replaced with a stop button)

Alternatively we could just replace pause with cancel/abort, or does pause offer any benefit over cancel?

[jeremysalwen]

With just this PR there is no way in the GUI to cancel. You either have to physically press the buttons on your mower, or run a script over ssh to send the message. However the GUI could be updated to trigger the cancellation with whatever UI is desirable.

The reason I chose the home and play buttons for the physical buttons, is because those are the only physical buttons that work on my Yardforce 500B running Mowgli. But if Cedric wants to rationalize the behavior of the buttons to make them more consistent, I would be fine with that.

[rovo89]

I skimmed over your commit and it looks quite good to me. Not sure if the logging and stopMoving() would be duplicated, once in approach_docking_point() and once in execute()?

Apart from that, replacing sendGoalAndWait() with the loop etc. results in a lot of duplicated/similar boilerplate code. Not sure whether cancelGoal() could be called from another thread while sendGoalAndWait() is running, and if that thread could be informed about the abort()... Or the simpler solution: Introduce a helper function like waitForResultOrAborted() to have that loop logic just once.

[jeremysalwen]

Yeah I am not familiar with the code style for ROS. It does seem like something cross-cutting could be added to allow aborting (e.g. another thread interrupting it, or exception throwing to easily abort execution).

I do like the idea of waitForResultOrAborted(), as the state can be checked and handled after the waiting is done in an independent way.

[rovo89]

Yeah, that could easily go into mower_logic.cpp and should return the full result as before - then it's up to the caller to evaluate why the goal failed. Will need some fiddling to get the parameter/return types magic correct, actionlib relies on templates for that. If you need help with that, I can give it a try tomorrow evening.

[jeremysalwen]

Okay, I added a new helper function to do the waiting and the code is a bit simpler now (also removed the double logging of aborting).

[jeremysalwen]

Hmm it seems like there are background abort attempts on the docking behavior which are now causing the docking to abort randomly. I will need to investigate this more before we merge.

[rovo89]

the code is a bit simpler now

Yes, indeed, I like that 😉 Thanks for implementing the suggestion!

there are background abort attempts on the docking behavior which are now causing the docking to abort randomly

I created this overview a few days ago: https://wiki.openmower.de/index.php?title=Why_is_the_mower_not_mowing%3F
Any idea if some of those reasons could trigger an abort(), which was previously ignored by DockingBehavior, but now sends it to IdleBehavior? Or is that not what you meant?

[rovo89]

Was this code style change intended? I thought this should be denied by the .clang-format file.

[jeremysalwen]

This was done by /pre-commit run --all-files, I didn't touch it.

[rovo89]

That's strange 😲 I just modified the file as in your commit and ran that exact command, it complained and corrected it back to what it was before:

clang-format.............................................................Failed
- hook id: clang-format
- exit code: 1
- files were modified by this hook

src/mower_logic/src/mower_logic/behaviors/Behavior.h
====================
--- original

+++ formatted

@@ -83,7 +83,7 @@

     requested_pause_flag |= reason;
   }

-  void start(mower_logic::MowerLogicConfig& c, std::shared_ptr<sSharedState> s) {
+  void start(mower_logic::MowerLogicConfig &c, std::shared_ptr<sSharedState> s) {
     ROS_INFO_STREAM("");
     ROS_INFO_STREAM("");
     ROS_INFO_STREAM("--------------------------------------");

[jeremysalwen]

Oof, conflicting clang-format versions maybe?

$ clang-format --version
Debian clang-format version 14.0.6

[rovo89]

Exact same for me. Did you try clang-format -n src/mower_logic/src/mower_logic/behaviors/Behavior.h? It emits two warnings for me with MowerLogicConfig& c, and changes it to MowerLogicConfig &c with -i instead of -n.

[jeremysalwen]

clang-format -n src/mower_logic/src/mower_logic/behaviors/Behavior.h gives no warning for me 🤦

[jeremysalwen]

@rovo89 Still getting the behavior where it quits docking... based on the log messages it looks like the goal is failing rather than getting aborted. Poked around a bit in the code... the only thing I could find that might be a lead is that the original code (sendGoalAndWait) was ultimately checking SimpleGoalState (a 3 choice enum) https://docs.ros.org/en/diamondback/api/actionlib/html/classactionlib_1_1SimpleGoalState.html and now we are instead checking simpleclientgoalstate (a 5 choice enum) https://docs.ros.org/en/diamondback/api/actionlib/html/classactionlib_1_1SimpleClientGoalState.html

However in dock_straight, we use the same approach... but with a slightly different client:

extern actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction> *mbfClient;
extern actionlib::SimpleActionClient<mbf_msgs::ExePathAction> *mbfClientExePath;

[rovo89]

Strange... did you find a pattern when this occurs, so you can reproduce it?

[rovo89]

Some more logging would help to verify that the goal state isn't success. With some special settings in rosconsole.config, you can enable debug logs for actionlib, maybe that would give more insights.

[jeremysalwen]

It's quite reliably reproducible for me. I see the error message "Error during docking approach." which implies it was not aborted, since the function would have returned before that point:

  bool approachSuccess = approach_docking_point();

  if (aborted) {
    ROS_INFO_STREAM("Docking aborted.");
    stopMoving();
    return &IdleBehavior::INSTANCE;
  }

  if (!approachSuccess) {
    ROS_ERROR("Error during docking approach.");

Likewise I do not see the message "Executing Docking Approach", so I know it was failing in the first part of approach_docking_point() (which logically makes sense since it happens shortly after I tell it to go dock).

I probably need extensive logging, which is annoying since I don't have a good setup for editing-compiling-running on my mower without waiting for docker images to build 🤦

[rovo89]

It's definitely worth investing some time into a more development-friendly setup. I build my own container with just the dependencies and mount /opt/open_mower_ros from the host. That way I can build incrementally (within the container) and use the same container to run it. Super easy and fast to have my specialized build and keep it in sync with upstream. I went one step further and switched to Docker Compose to run nginx and mosquitto in separate containers with their official images. My scripts are here, unfortunately I didn't get around to write a README yet: https://github.com/rovo89/open_mower_bare_container

[jeremysalwen]

eheh, so it was just a me being stupid problem. I needed to run docker compose pull before docker compose up to get the latest version of the image... But yeah I probably should invest in a better dev env.

[rovo89]

So does it work now for you? Or still seeing spurious failed actions?

[jeremysalwen]

It works for me now, I was just running an outdated version of the code.

[rovo89]

Nice! I'll also try it then.

One thought: The actions are named differently depending on the current behavior. Would that be a problem for clients, would it make it much easier for them if there was a "mower_logic/abort" alias? Maybe now would be a good time to prepare the app to show the stop button also for docking and undocking, that will show whether it's complicated or not.

[jeremysalwen]

Nice! I'll also try it then.

One thought: The actions are named differently depending on the current behavior. Would that be a problem for clients, would it make it much easier for them if there was a "mower_logic/abort" alias? Maybe now would be a good time to prepare the app to show the stop button also for docking and undocking, that will show whether it's complicated or not.

Yeah, I wasn't sure how much I should change the existing style of things, but that sounds more logical to me. Additionally, I need to add support for interrupting the "paused" mode when it loses a GPS fix.

[rovo89]

Additionally, I need to add support for interrupting the "paused" mode when it loses a GPS fix.

UndockingBehavior::waitForGPS() checks for aborted already - except when waiting gps_wait_time after it already got the fix. Not sure how immediate you want it to react in that case, maybe it's fine to just wait those few seconds.

[jeremysalwen]

Additionally, I need to add support for interrupting the "paused" mode when it loses a GPS fix.

UndockingBehavior::waitForGPS() checks for aborted already - except when waiting gps_wait_time after it already got the fix. Not sure how immediate you want it to react in that case, maybe it's fine to just wait those few seconds.

I think the issue is in MowingBehavior, technically not related to docking/undocking aborting, but really the goal of this pr was to be able to get to idle from ANY state.

[rovo89]

Ah sorry, I misread. I thought you were talking about the initial wait for a GPS fix.