Orchestration Needs: Global state machine for gathering apples in orchard

[adamdbrw]

This could be realized by scripting or by a custom (purpose-built) ROS 2 node.
We will handle only one robot with this orchestration, but it should be done in a flexible way to accommodate for multiple robots (and not run into conflicts at least for some time, e.g. for entire row).

It would be best if this orchestration was robust enough to pick operation from any valid point.

Is Simulation running -> Await simulation start

  flowchart TD;
      A[Start]-->B{Is simulation running?};
      B -- No --> C[Wait for simulation start]
      C --> B
      B -- Yes -->D{Is robot spawned?};
      D -- No -->E[Spawn Robot];
      D -- Yes --> F{Is robot in a gathering position?};
      E --> D;
      F -- No --> G[Navigate to the closest gathering position - Start Point];
      F -- Yes --> H[Gather all apples];
      G --> F;
      H --> I{Is finished globally?};
      I -- Yes --> Z[Terminate or reset];
      I -- No --> G;

Notes:
Run apples task - it is useful to add a timeout in case we can't gather all apples.

[j-rivero]

We will handle only one robot with this orchestration, but it should be done in a flexible way to accommodate for multiple robots (and not run into conflicts at least for some time, e.g. for entire row).

Let's keep in mind that having multiple mobile robots con lead to situations of collapse where, after some movements, the robots deal with a difficult situation to be resolved by the nav2 stack. Even assuming that we are going to use the recovery strategies available in the nav2 framework.

For spawning multiple robots we probably want to be sure that all them are in the scene before starting. Also to give them different initial positions to avoid the collapse situation described in above.

For a multiple robot scenario we also want to consider that a robot could possibly navigate from "Navigate to the closest gathering position" to "Is finished globally?" is other robots have finished all the work and there is no more gathering position to adopt.

Not sure how we are going to script the goals of each robot in the scene but we might need to add a "calculate navigation points for the robot" that can encapsulate a bit of logic to handle multi-robot/single-robot goals delivering. For a single robot this is pretty straightforward, for a multi-robot scenario the logic to determine the "new gathering spot" could be more interesting. Several strategies can be follow, probably having a global dispatcher can be the more robust approach with hardcoded positions for each of the trees.

Run apples task - it is useful to add a timeout in case we can't gather all apples.

+10. We can improve the timeout check but checking if robot/s are not increasing the total number of apples picked in a given time instead of computing the total demo time or even check if the robot is indeed moving since sometimes navigation are slow but successful, long but funny and people hate to see the fun spectacle stopped. I think that this can be added to this general diagram.

[adamdbrw]

@j-rivero Good points!

This state machine is focused on a single robot scenario. I assumed we could run multiple of these in a way that they do not interfere or know about each other, for example:

1. One robot for each row - is finished "globally" means that its row is finished.
2. One robot for a couple of rows (exclusively), etc.

"Terminate or reset" could be renamed to "Finish: stop" in this case. What do you think?
We could add a check whether all robots finished and then reset or terminate.

[j-rivero]

"Terminate or reset" could be renamed to "Finish: stop" in this case. What do you think? We could add a check whether all robots finished and then reset or terminate.

Split the problem and give a per robot a given set of apple raws sounds like a good approach to simplify the problem to me. Good idea. It can lead to a situation where someone would expect a different behavior (a robot is stopped while another one is still slow collection apples) but to start with seems pretty valid to me.

[adamdbrw]

We are currently working on it