Only the first found plan is taken into account

[teyssieuman]

The TFD solver is parameter in such a way that it give several plan in an increasing 'quality' order :

a - enable anytime search (otherwise finish on first plan found)

https://github.com/IntelligentRoboticsLabs/plansys2_tfd_plan_solver/blob/55d99752b0357d35d65d10c884092da34df9371c/src/plansys2_tfd_plan_solver/tfd_plan_solver.cpp#L92

But later, only the first found plan is taken into account:

https://github.com/IntelligentRoboticsLabs/plansys2_tfd_plan_solver/blob/55d99752b0357d35d65d10c884092da34df9371c/src/plansys2_tfd_plan_solver/tfd_plan_solver.cpp#L97

[fmrico]

Hi @teyssieuman

Yes. The idea is to have a plan as soon as possible to have a reactive system.

A nice, but complex improvement is to change the executing plan as soon as you find a better plan. Not only waiting for a better plan; new perceptions of the environment, translated to predicates, could produce better plans for achieving the final goal. But there are many open questions. For example: Should we abort executing actions? It depends if they are at the initial of the new plan.

Do you propose a better option than the current one?

Best

[teyssieuman]

As you said, using a result as soon as they are available seem to be a good idea in the case of reactive system.
In my experimentations, it seems that:

1. Some better results can be available soon after the first one, sometime one or two seconds, which is not so long in relatively to the actions duration in most robotics applications.
2. It seems that the plans that are found after the first one, do not differs from the first one in many cases (I thing that this is due to the uses of heuristics to perform depth first search.

Considering those two points, a strategy can consist on:

• Take the first plan as a result, and immediately execute it (we can add “wait” a parameter, in case a better result is found within a certain amount of time after the launch)
• Let the planner run, until it found a new result, or it is asked for a new solution (or something change in the environment)
• If a better plan is found AND, it is compatible with the current plan execution (the initial steps are the same, and the steps that differs are not executed yet, then we execute this better plan instead of the first one.
• In case the better plan is not compatible with the current execution, then give the currently execution action end state as a new starting point. If a better path is found, then get back to the previous point.
  I hope that those ideas do not look as a specification. This can be the case due to my professional activities, but it only aim to share ideas. Does it correspond to the ones you have?

Cordialmente …

[fmrico]

Ok. I see your point and I agree with your view. One way to implement this could be:

• Executor must deal with the existence of a new different/improved plan. The change of plan should be developed there, but first, we should solve the first part: continuously receiving a plan, even if the executor is already executing one.
• Executor uses a service call to get the plan from the planner. Maybe Executor could be continuously asking for a new plan. The option is to use a publisher instead of a service.
• If using service, and the executor continuously asks for a plan. The planner could return the last plan calculated, taking into account that it could be improved.

As soon as we solve this, we could work on the change of the plan in the executor. Are you planning on providing a solution for the above improvement?

[teyssieuman]

I'm afraid to not be familiar enough with ros2 architectures to be pertinent with this choices. But isn't it possible to use a simple scalar integer topic that continuously stream the number of plan found for a given problem : 0 for no plan, 1 for the first ...
The Executor can then monitor this state and ark for a new plan when is is available.
Then, two service can be defined :

1. launch the problem resolution
2. get a plan (parameterized by the plan number)

I can try to help solving the above issue since I can have use of such a strategy.