Improvements to the evaluate function focusing on optimizing derived predicates and existential preconditions resolution

[Rezenders]

This PR is a draft to solve issue #328.

I implemented the algorithm described by @tobiaswjohn for most of the Plansys2 Node types.

This version is much faster than the previous version in the main branch. It takes around 37643ms to solve all derived predicates in this PDDL formulation, whereas it wouldn't finish with the previous version of the algorithm.

The evaluate function could be further improved by:

• by feeding the already assigned variables to the eval function
• with an algorithm that checks the type of the derived predicate parameters and finds out the possible types for the variables in the derived predicate preconditions, which could reduce the search space significantly in certain cases

I still need to adjust the other packages to make it compatible with the new evaluation and problem expert.

I didn't adjust the evaluation for anything related to functions. If you think this approach would be mergeable when complete, I can adjust it to handle functions. But right now, I don't need it for my project, so I will focus on finishing what I need first.

[Rezenders]

Possible improvements:

• Move StateVec to the plansys2_problem_expert (or plansys2_core) package and add a StateVect getState() method to problemExpert. Then use the StateVec as an input to the evaluation function instead of passing instances, predicates and functions
• Do we really need to get the state and pass it to the evaluation? Can't we just use the problemExperClient? It would make things much simpler

[Rezenders]

When the evaluate function is called with apply=true and use_state=true, the derived predicates are not recalculated after the state is modified. Check: https://github.com/Rezenders/ros2_planning_system/blob/b6c996cc2be1c993c8cbfb80c24e09e758599998/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp#L105-L113

This is a crucial issue when generating the plan graph.
To solve this problem, I propose creating a new State class containing the instances, functions, predicates, inferred_predicates, and derived predicates.

class State
  instances
  predicates
  inferred_predicates
  derived_predicates
  functions

Then:

1. pass an instance of State to the evaluate function instead of passing the instances, functions, and predicates
2. after an apply operation, clear the inferred_predicates and use the derived_predicates to recalculate them
3. add a State msg to plansys2_msgs
4. add a getState method to the ProblemExpert
5. add a get_state service to the ProblemExpertNode
6. add a getState method to the ProblemExpertClient

evaluate(tree, problem_client, state, apply, use_state, node_id, negate):
...
case Predicate:
 if(apply):
   if(use_state):
     apply_operation(state.predicates)
     state.inferred_predicates.clear()
     state = all_derived(state)
...

[Rezenders]

To solve this problem, I propose creating a new State class containing the instances, functions, predicates, inferred_predicates, and derived predicates.

Actually, I think the State class shouldn't contain derived_predicates. The derived_predicates would only be needed on apply operations. Maybe it is better to separate the apply function from the evaluate function, and pass the derived_predicates to the apply function, e.g., apply(tree, problem_client, state, derived_predicates). It would make the evaluate function more readable. However, I don't know how much code would be repeated in both.

class State
  instances
  predicates
  inferred_predicates
  functions

[fmrico]

Sorry for the conflicts, @Rezenders . There were some things to fix

Please, merge, or better, rebase

[Rezenders]

Ideas to improve BT generator algorithm:

1. cache the resulting state of applying an action to a State, e.g., cache[state, action] = apply(action.effects, state). This would probably require hashing a State individual which I believe has complexity O(size(State))
2. early return in get_node_satisfy when the first satisfying node is found. Here:

   ros2_planning_system/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp

   Lines 110 to 112 in ab59c45

   	if (satisfied_after && !satisfied_before) {
   	ret = node;
   	}

   
   Also here:

   ros2_planning_system/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp

   Line 209 in ab59c45

   ret = node_satisfy;

   
   Does this make sense?
3. Check if the get_node_satisfy and get_node_contradict loops can be merged to avoid unnecessary loops

[Rezenders]

Sorry for the conflicts, @Rezenders . There were some things to fix

Please, merge, or better, rebase

Ok, no worries :)

[Rezenders]

Adding an <action, state> cache reduced the time for creating a graph from around 145 seconds to 13 seconds in the following test:
https://github.com/Rezenders/ros2_planning_system/blob/da7feb9d1ff92cdc5b25f9c693f7b29ec8972a22/plansys2_executor/test/unit/simple_btbuilder_tests.cpp#L958

[Rezenders]

The derived predicates graph algorithm described in #328 is implemented. The time for creating the plan graph is now around 1.3 seconds.
I also changed the BT nodes to solve the derived predicates when needed. The whole planning->plan graph generation->plan execution pipeline is now working. I added multiple unit tests and I tested it in simulation with SUAVE.

Some improvements can still be made but I will work on it later. For example, adding a <action, state> cache in the BT nodes.

I am sorry that the PR got super long, it will probably be annoying to review it.