Orchestration Needs - apple gathering

[adamdbrw]

Related to #42 since it realizes Gather all apples block.

Some characteristics of this orchestration:

1. Does not care about any kind of movement or navigation.
2. Assumes it is always at a gathering point when ran.
3. It is finished when all reachable apples were gathered (optionally, also on a timeout).
4. It will work well when started in a middle of picking or even with manipulator holding the apple (in a little box).

  flowchart TD;
      A[Wait for work]-->B{Task received?};
      B -- No --> A;
      B -- Yes --> Q[Queue all reachable apples];
      Q --> C{Is apple held by/in manipulator?};
      C -- Yes --> D{Is manipulator in storage position?};
      D -- Yes --> E[Store the apple / handle full storage condition];
      D -- No --> F[Move manipulator to storage position];
      F --> D;
      C -- No --> H{Apple queue empty?};
      E --> H;
      H -- Yes --> Z[Notify that task is completed];
      H -- No --> I[Pop apple pick task from the queue];
      I --> J{Is manipulator in apple pick up position for current apple?}
     J -- No --> K[Move manipulator to picking position for current apple];
     K --> J;
     J -- Yes --> L[Attempt to pick apple];
     L --> M{Was picking successful?}
     M -- Yes / timeout / attempts limit --> C;
     M -- No --> L; 
     Z --> A;

[j-rivero]

Not sure if we are going to use MoveIt here or other software handling movements. In that case we probably need to interact with it to know if a movement is being done or not in the same way we interact with nav2.

I don't see any recovery/fail strategy described, for both movements problems of the arm and picking. For picking if we go with teleportation given a desired Pose that can be omitted. For movement a basic strategy of maximum tries and go to the next implemented in "Was picking successful?" could be enough.

There is no mention to the input per tree already pre-computed, we probably want a list of Pose (Orientation + Position).

For a multi-robot env, we probably want to assume that no two robots get into the same apple gathering position.

[adamdbrw]

I also thought it could be enough, since we are basing it on ground truth.
As for pre-computing, there could be issues since we would not be in exactly the same spot (also, tilt a bit due to terrain), but it is a possibility. I think it is not computationally expensive to do that once we are in a spot (it is done once per tree).

For multi-robot, I assumed exactly that - otherwise we would need to solve more problems (two robots approaching each other etc). But if a robot comes to an empty apple tree the state machine would just complete the work after two checks (apple queue empty, no apple in manipulator).

[j-rivero]

As for pre-computing, there could be issues since we would not be in exactly the same spot (also, tilt a bit due to terrain), but it is a possibility. I think it is not computationally expensive to do that once we are in a spot (it is done once per tree).

Yep, you are right that the robot position could not be the same and that can change the pick Pose a bit. How are we going to compute the Pose given the robot position? I assume that the script knows about the apple positions on each tree, is this right?

[adamdbrw]

Exactly, we will use ground truth position of apples. This could be replaced by a detector if someone wishes to take this demo and use it for something more realistic