[WIP] Plansys2 problem expert

plansys2_core/include/plansys2_core/State.hpp

@@ -248,48 +248,48 @@ class State
     return functions_.erase(std::move(function)) == 1;
   }
 
-  bool hasInstance(const plansys2::Instance & instance)
+  bool hasInstance(const plansys2::Instance & instance) const
   {
     return instances_.find(instance) != instances_.end();
   }
-  bool hasInstance(plansys2::Instance && instance)
+  bool hasInstance(plansys2::Instance && instance) const
   {
     return instances_.find(std::move(instance)) != instances_.end();
   }
 
-  bool hasPredicate(const std::string & predicate_str)
+  bool hasPredicate(const std::string & predicate_str) const
   {
     auto predicate = parser::pddl::fromStringPredicate(predicate_str);
     return predicates_.find(predicate) != predicates_.end();
   }
-  bool hasPredicate(const plansys2::Predicate & predicate)
+  bool hasPredicate(const plansys2::Predicate & predicate) const
   {
     return predicates_.find(predicate) != predicates_.end();
   }
-  bool hasPredicate(plansys2::Predicate && predicate)
+  bool hasPredicate(plansys2::Predicate && predicate) const
   {
     return predicates_.find(std::move(predicate)) != predicates_.end();
   }
 
-  bool hasInferredPredicate(const std::string & predicate_str)
+  bool hasInferredPredicate(const std::string & predicate_str) const
   {
     auto predicate = parser::pddl::fromStringPredicate(predicate_str);
     return inferred_predicates_.find(predicate) != inferred_predicates_.end();
   }
-  bool hasInferredPredicate(const plansys2::Predicate & predicate)
+  bool hasInferredPredicate(const plansys2::Predicate & predicate) const
   {
     return inferred_predicates_.find(predicate) != inferred_predicates_.end();
   }
-  bool hasInferredPredicate(plansys2::Predicate && predicate)
+  bool hasInferredPredicate(plansys2::Predicate && predicate) const
   {
     return inferred_predicates_.find(std::move(predicate)) != inferred_predicates_.end();
   }
 
-  bool hasFunction(const plansys2::Function & function)
+  bool hasFunction(const plansys2::Function & function) const
   {
     return functions_.find(function) != functions_.end();
   }
-  bool hasFunction(plansys2::Function && function)
+  bool hasFunction(plansys2::Function && function) const
   {
     return functions_.find(std::move(function)) != functions_.end();
   }
@@ -315,6 +315,18 @@ class State
     return functions_.find(std::move(function));
   }
 
+  bool updateFunctionValue(const plansys2::Function & function, const double & value)
+  {
+    auto func_it = functions_.find(function);
+    if (func_it != functions_.end()) {
+      plansys2::Function updated_func = *func_it;
+      removeFunction(func_it);
+      updated_func.value = value;
+      return addFunction(updated_func);
+    }
+    return false;
+  }
+
   void setDerivedPredicates(const plansys2::DerivedResolutionGraph derived_predicates)
   {
     derived_predicates_ = derived_predicates;

plansys2_executor/CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(pluginlib REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(OpenMP REQUIRED)
 
 set(EXECUTOR_SOURCES
   src/plansys2_executor/ExecutorClient.cpp
@@ -61,6 +62,7 @@ target_link_libraries(${PROJECT_NAME}
   rclcpp_action::rclcpp_action
   rclcpp_cascade_lifecycle::rclcpp_cascade_lifecycle
   rclcpp_lifecycle::rclcpp_lifecycle
+  OpenMP::OpenMP_CXX
   ${std_msgs_TARGETS}
   ${std_srvs_TARGETS}
   PRIVATE

plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp

@@ -21,6 +21,7 @@
 #include <set>
 #include <list>
 #include <map>
+#include <unordered_map>
 #include <utility>
 #include <tuple>
 
@@ -35,6 +36,29 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 
+namespace std
+{
+template<typename T1, typename T2>
+struct hash<std::pair<T1, T2>>
+{
+  std::size_t operator()(const std::pair<T1, T2> & p) const
+  {
+    std::size_t seed = 0;
+    hash_combine(seed, p.first);
+    hash_combine(seed, p.second);
+    return seed;
+  }
+
+private:
+  // Hash combine function
+  template<typename T>
+  inline void hash_combine(std::size_t & seed, const T & value) const
+  {
+    seed ^= std::hash<T>{}(value) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+  }
+};
+}  // namespace std
+
 namespace plansys2
 {
 
@@ -53,8 +77,7 @@ struct ActionNode
   int node_num;
   int level_num;
 
-  std::vector<plansys2::Predicate> predicates;
-  std::vector<plansys2::Function> functions;
+  plansys2::State state;
 
   std::list<ActionNode::Ptr> in_arcs;
   std::list<ActionNode::Ptr> out_arcs;
@@ -181,6 +204,11 @@ class SimpleBTBuilder : public BTBuilder
    */
   void prune_forward(ActionNode::Ptr current, std::list<ActionNode::Ptr> & used_nodes);
 
+  void get_state_recursive(
+    const ActionNode::Ptr & node,
+    std::list<ActionNode::Ptr> & used_nodes,
+    plansys2::State & state);
+
   /**
    * @brief Computes the state of the world at a node.
    *
@@ -189,41 +217,44 @@ class SimpleBTBuilder : public BTBuilder
    *
    * @param[in] node The node to compute state for.
    * @param[in,out] used_nodes List of nodes already processed.
-   * @param[out] predicates Resulting predicates at this node.
-   * @param[out] functions Resulting functions at this node.
+   * @param[out] state Resulting state at this node.
    */
-  void get_state(
+  plansys2::State get_state(
     const ActionNode::Ptr & node,
     std::list<ActionNode::Ptr> & used_nodes,
-    std::vector<plansys2::Predicate> & predicates,
-    std::vector<plansys2::Function> & functions) const;
+    const plansys2::State & state);
 
   /**
    * @brief Checks if an action is executable in a given state.
    *
    * @param[in] action The action to check.
-   * @param[in] predicates Current predicates.
-   * @param[in] functions Current functions.
+   * @param[in] state Current state.
    * @return True if the action's requirements are satisfied, false otherwise.
    */
   bool is_action_executable(
     const ActionStamped & action,
-    std::vector<plansys2::Predicate> & predicates,
-    std::vector<plansys2::Function> & functions) const;
+    const plansys2::State & state) const;
+
+  std::vector<plansys2_msgs::msg::Tree> check_requirements(
+    const std::vector<plansys2_msgs::msg::Tree> & requirements,
+    std::shared_ptr<plansys2::ActionGraph> & graph,
+    std::shared_ptr<plansys2::ActionNode> & new_node);
+  bool check_requirement(
+    const plansys2_msgs::msg::Tree & requirement,
+    std::shared_ptr<plansys2::ActionGraph> & graph,
+    std::shared_ptr<plansys2::ActionNode> & new_node);
 
   /**
    * @brief Finds root actions that can be executed immediately.
    *
    * @param[in,out] action_sequence Actions to check, executable ones will be removed.
-   * @param[in] predicates Current predicates.
-   * @param[in] functions Current functions.
+   * @param[in] state Current state.
    * @param[in,out] node_counter Counter for assigning unique node IDs.
    * @return std::list<ActionNode::Ptr> List of nodes representing executable actions.
    */
   std::list<ActionNode::Ptr> get_roots(
     std::vector<plansys2::ActionStamped> & action_sequence,
-    std::vector<plansys2::Predicate> & predicates,
-    std::vector<plansys2::Function> & functions,
+    const plansys2::State & state,
     int & node_counter);
 
   /**
@@ -279,28 +310,28 @@ class SimpleBTBuilder : public BTBuilder
    * @brief Removes requirements that are already satisfied in the current state.
    *
    * @param[in,out] requirements List of requirements to filter.
-   * @param[in] predicates Current predicates.
-   * @param[in] functions Current functions.
+   * @param[in] state Current state.
    */
   void remove_existing_requirements(
     std::vector<plansys2_msgs::msg::Tree> & requirements,
-    std::vector<plansys2::Predicate> & predicates,
-    std::vector<plansys2::Function> & functions) const;
+    const plansys2::State & state) const;
 
   /**
    * @brief Checks if an action can be executed in parallel with a set of other actions.
    *
    * @param[in] action The action to check.
-   * @param[in] predicates Current predicates.
-   * @param[in] functions Current functions.
+   * @param[in] state Current state.
    * @param[in] ret List of actions to check parallelization with.
    * @return True if the action can be executed in parallel, false otherwise.
    */
   bool is_parallelizable(
     const plansys2::ActionStamped & action,
-    const std::vector<plansys2::Predicate> & predicates,
-    const std::vector<plansys2::Function> & functions,
-    const std::list<ActionNode::Ptr> & ret) const;
+    const plansys2::State & state,
+    const std::list<ActionNode::Ptr> & ret);
+
+  void apply_action_to_state(
+    const plansys2::ActionStamped & action,
+    plansys2::State & state);
 
   /**
    * @brief Generates the behavior tree XML for a node and its descendants.
@@ -436,6 +467,10 @@ class SimpleBTBuilder : public BTBuilder
   ActionGraph::Ptr graph_;
   std::string bt_;
   std::string bt_action_;
+
+  std::unordered_map<
+    std::pair<std::string, plansys2::State>, plansys2::State> apply_action_state_cache_;
+  std::unordered_map<std::pair<std::string, plansys2::State>, bool> check_action_state_cache_;
 };
 
 }  // namespace plansys2

plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp

@@ -26,6 +26,7 @@
 #include <utility>
 #include <vector>
 
+#include "plansys2_core/State.hpp"
 #include "plansys2_domain_expert/DomainExpertClient.hpp"
 #include "plansys2_executor/ActionExecutor.hpp"
 #include "plansys2_executor/BTBuilder.hpp"
@@ -35,18 +36,6 @@
 namespace plansys2
 {
 
-/**
- * @brief Structure that represents a state vector in the planning system.
- *
- * Contains the predicates and functions that define a state of the world
- * at a specific point in time during plan execution.
- */
-struct StateVec
-{
-  std::vector<plansys2::Predicate> predicates;
-  std::vector<plansys2::Function> functions;
-};
-
 /**
  * @class plansys2::STNBTBuilder
  * @brief Behavior tree builder that uses Simple Temporal Networks (STN) to manage temporal constraints.
@@ -199,9 +188,9 @@ class STNBTBuilder : public BTBuilder
    *
    * @param[in] happenings The set of happening times.
    * @param[in] plan The simplified plan representation.
-   * @return std::map<int, StateVec> Map from happening times to state vectors.
+   * @return std::map<int, plansys2::State> Map from happening times to state vectors.
    */
-  std::map<int, StateVec> get_states(
+  std::map<int, plansys2::State> get_states(
     const std::set<int> & happenings,
     const std::multimap<int, ActionStamped> & plan) const;
 
@@ -213,8 +202,7 @@ class STNBTBuilder : public BTBuilder
    * @return Tree representing the conjunction of predicates and functions.
    */
   plansys2_msgs::msg::Tree from_state(
-    const std::vector<plansys2::Predicate> & preds,
-    const std::vector<plansys2::Function> & funcs) const;
+    const plansys2::State & state) const;
 
   /**
    * @brief Finds the graph nodes corresponding to an action.
@@ -252,7 +240,7 @@ class STNBTBuilder : public BTBuilder
     const std::pair<int, ActionStamped> & action,
     const std::multimap<int, ActionStamped> & plan,
     const std::set<int> & happenings,
-    const std::map<int, StateVec> & states) const;
+    const std::map<int, plansys2::State> & states) const;
 
   /**
    * @brief Finds actions that satisfy the requirements of an action.
@@ -267,7 +255,7 @@ class STNBTBuilder : public BTBuilder
     const std::pair<int, ActionStamped> & action,
     const std::multimap<int, ActionStamped> & plan,
     const std::set<int> & happenings,
-    const std::map<int, StateVec> & states) const;
+    const std::map<int, plansys2::State> & states) const;
 
   /**
    * @brief Finds actions that threaten the execution of an action.
@@ -285,7 +273,7 @@ class STNBTBuilder : public BTBuilder
     const std::pair<int, ActionStamped> & action,
     const std::multimap<int, ActionStamped> & plan,
     const std::set<int> & happenings,
-    const std::map<int, StateVec> & states) const;
+    const std::map<int, plansys2::State> & states) const;
 
   /**
    * @brief Checks if an action can be applied in a given state.
@@ -303,7 +291,7 @@ class STNBTBuilder : public BTBuilder
     const std::pair<int, ActionStamped> & action,
     const std::multimap<int, ActionStamped> & plan,
     const int & time,
-    StateVec & state) const;
+    plansys2::State & state) const;
 
   /**
    * @brief Computes the difference between two states.
@@ -312,9 +300,9 @@ class STNBTBuilder : public BTBuilder
    *
    * @param[in] X_1 The first state.
    * @param[in] X_2 The second state.
-   * @return StateVec containing the differences.
+   * @return plansys2::State containing the differences.
    */
-  StateVec get_diff(const StateVec & X_1, const StateVec & X_2) const;
+  plansys2::State get_diff(const plansys2::State & X_1, const plansys2::State & X_2) const;
 
   /**
    * @brief Computes the intersection of two states.
@@ -323,9 +311,9 @@ class STNBTBuilder : public BTBuilder
    *
    * @param[in] X_1 The first state.
    * @param[in] X_2 The second state.
-   * @return StateVec containing the intersection.
+   * @return plansys2::State containing the intersection.
    */
-  StateVec get_intersection(const StateVec & X_1, const StateVec & X_2) const;
+  plansys2::State get_intersection(const plansys2::State & X_1, const plansys2::State & X_2) const;
 
   /**
    * @brief Gets the conditions required by an action.