⚡ new evaluate function (issue PlanSys2#328)

Rezenders · Oct 24, 2024 · 485bb5b · 485bb5b
1 parent 424ffb8
commit 485bb5b
Show file tree

Hide file tree

Showing 18 changed files with 1,505 additions and 868 deletions.
diff --git a/plansys2_problem_expert/CMakeLists.txt b/plansys2_problem_expert/CMakeLists.txt
@@ -14,6 +14,8 @@ find_package(plansys2_domain_expert REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 
+find_package(OpenMP REQUIRED)
+
 set(CMAKE_CXX_STANDARD 17)
 
 set(dependencies
@@ -27,6 +29,7 @@ set(dependencies
     plansys2_domain_expert
     std_msgs
     lifecycle_msgs
+    OpenMP
 )
 
 include_directories(include)
@@ -41,12 +44,21 @@ set(PROBLEM_EXPERT_SOURCES
 add_library(${PROJECT_NAME} SHARED ${PROBLEM_EXPERT_SOURCES})
 ament_target_dependencies(${PROJECT_NAME} ${dependencies})
 
+# # Link OpenMP to the target
+if(OpenMP_CXX_FOUND)
+  target_link_libraries(${PROJECT_NAME} OpenMP::OpenMP_CXX)
+endif()
+
 add_executable(problem_expert_node
   src/problem_expert_node.cpp
 )
 ament_target_dependencies(problem_expert_node ${dependencies})
 target_link_libraries(problem_expert_node ${PROJECT_NAME})
 
+if(OpenMP_CXX_FOUND)
+  target_link_libraries(problem_expert_node OpenMP::OpenMP_CXX)
+endif()
+
 install(DIRECTORY include/
   DESTINATION include/
 )

diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp
@@ -36,12 +36,19 @@ class ProblemExpert : public ProblemExpertInterface
 public:
   explicit ProblemExpert(std::shared_ptr<DomainExpert> & domain_expert);
 
-  std::vector<plansys2::Instance> getInstances();
+  std::unordered_set<plansys2::Instance> getInstances();
   bool addInstance(const plansys2::Instance & instance);
   bool removeInstance(const plansys2::Instance & instance);
   std::optional<plansys2::Instance> getInstance(const std::string & name);
 
-  std::vector<plansys2::Predicate> getPredicates();
+  void groundPredicate(
+    std::unordered_set<plansys2::Predicate>& current_predicates,
+    const plansys2::Predicate& predicate,
+    const std::vector<std::map<std::string, std::string>>& params_values_vector);
+  std::unordered_set<plansys2::Predicate> solveDerivedPredicates(std::unordered_set<plansys2::Predicate>& predicates);
+  std::unordered_set<plansys2::Predicate> solveAllDerivedPredicates(const std::unordered_set<plansys2::Predicate>& predicates);
+
+  std::unordered_set<plansys2::Predicate> getPredicates();
   bool addPredicate(const plansys2::Predicate & predicate);
   bool removePredicate(const plansys2::Predicate & predicate);
   bool existPredicate(const plansys2::Predicate & predicate);
@@ -77,15 +84,15 @@ class ProblemExpert : public ProblemExpertInterface
     uint8_t node_id = 0);
 
   void removeInvalidPredicates(
-    std::vector<plansys2::Predicate> & predicates,
+    std::unordered_set<plansys2::Predicate> & predicates,
     const plansys2::Instance & instance);
   void removeInvalidFunctions(
     std::vector<plansys2::Function> & functions,
     const plansys2::Instance & instance);
   void removeInvalidGoals(const plansys2::Instance & instance);
 
-  std::vector<plansys2::Instance> instances_;
-  std::vector<plansys2::Predicate> predicates_;
+  std::unordered_set<plansys2::Instance> instances_;
+  std::unordered_set<plansys2::Predicate> predicates_;
   std::vector<plansys2::Function> functions_;
   plansys2::Goal goal_;
 

diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp
@@ -50,12 +50,12 @@ class ProblemExpertClient : public ProblemExpertInterface
 public:
   ProblemExpertClient();
 
-  std::vector<plansys2::Instance> getInstances();
+  std::unordered_set<plansys2::Instance> getInstances();
   bool addInstance(const plansys2::Instance & instance);
   bool removeInstance(const plansys2::Instance & instance);
   std::optional<plansys2::Instance> getInstance(const std::string & name);
 
-  std::vector<plansys2::Predicate> getPredicates();
+  std::unordered_set<plansys2::Predicate> getPredicates();
   bool addPredicate(const plansys2::Predicate & predicate);
   bool removePredicate(const plansys2::Predicate & predicate);
   bool existPredicate(const plansys2::Predicate & predicate);

diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp
@@ -17,6 +17,7 @@
 
 #include <string>
 #include <vector>
+#include <unordered_set>
 
 #include "plansys2_msgs/msg/node.hpp"
 #include "plansys2_msgs/msg/param.hpp"
@@ -32,12 +33,12 @@ class ProblemExpertInterface
 public:
   ProblemExpertInterface() {}
 
-  virtual std::vector<plansys2::Instance> getInstances() = 0;
+  virtual std::unordered_set<plansys2::Instance> getInstances() = 0;
   virtual bool addInstance(const plansys2::Instance & instance) = 0;
   virtual bool removeInstance(const plansys2::Instance & instance) = 0;
   virtual std::optional<plansys2::Instance> getInstance(const std::string & name) = 0;
 
-  virtual std::vector<plansys2::Predicate> getPredicates() = 0;
+  virtual std::unordered_set<plansys2::Predicate> getPredicates() = 0;
   virtual bool addPredicate(const plansys2::Predicate & predicate) = 0;
   virtual bool removePredicate(const plansys2::Predicate & predicate) = 0;
   virtual bool existPredicate(const plansys2::Predicate & predicate) = 0;

diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp
@@ -30,10 +30,38 @@
 namespace plansys2
 {
 
+std::tuple<bool, std::vector<std::map<std::string, std::string>>> unifyPredicate(
+  const plansys2::Predicate& predicate,
+  const std::unordered_set<plansys2::Predicate>& predicates);
+
+std::tuple<bool, std::vector<std::map<std::string, std::string>>> unifyFunction(
+  const plansys2::Function& function,
+  const std::vector<plansys2::Function>& functions);
+
+void mergeParamsValuesDicts(
+  const std::map<std::string, std::string>& dict1,
+  const std::map<std::string, std::string>& dict2,
+  std::map<std::string, std::string>& dict3);
+std::vector<std::map<std::string, std::string>> mergeParamsValuesVector(
+  const std::vector<std::map<std::string, std::string>>& vector1,
+  const std::vector<std::map<std::string, std::string>>& vector2);
+std::vector<std::map<std::string, std::string>> complementParamsValuesVector(
+  const std::vector<plansys2_msgs::msg::Param> & params,
+  const std::vector<std::map<std::string, std::string>>& param_dict_vector,
+  const std::unordered_set<plansys2::Instance> & instances);
+
+std::tuple<bool, std::vector<std::map<std::string, std::string>>> negateResult(
+  const plansys2_msgs::msg::Node & node,
+  const bool &result,
+  const std::vector<std::map<std::string, std::string>>& param_dict_vector,
+  const std::unordered_set<plansys2::Instance> & instances
+);
+
 /// Evaluate a PDDL expression represented as a tree.
 /**
  * \param[in] node The root node of the PDDL expression.
  * \param[in] problem_client The problem expert client.
+ * \param[in] instances Current instances state.
  * \param[in] predicates Current predicates state.
  * \param[in] functions Current functions state.
  * \param[in] apply Apply result to problem expert or state.
@@ -43,26 +71,29 @@ namespace plansys2
  *         result(0) true if success
  *         result(1) truth value of boolean expression
  *         result(2) value of numeric expression
+ *         result(3) vector with the set of possible  values for the expression parameters
  */
-std::tuple<bool, bool, double> evaluate(
+std::tuple<bool, bool, double, std::vector<std::map<std::string, std::string>>>  evaluate(
   const plansys2_msgs::msg::Tree & tree,
   std::shared_ptr<plansys2::ProblemExpertClient> problem_client,
-  std::vector<plansys2::Predicate> & predicates,
+  std::unordered_set<plansys2::Instance> & instances,
+  std::unordered_set<plansys2::Predicate> & predicates,
   std::vector<plansys2::Function> & functions,
   bool apply = false,
   bool use_state = false,
   uint8_t node_id = 0,
   bool negate = false);
 
-std::tuple<bool, bool, double> evaluate(
+std::tuple<bool, bool, double, std::vector<std::map<std::string, std::string>>> evaluate(
   const plansys2_msgs::msg::Tree & tree,
   std::shared_ptr<plansys2::ProblemExpertClient> problem_client,
   bool apply = false,
   uint32_t node_id = 0);
 
-std::tuple<bool, bool, double> evaluate(
+std::tuple<bool, bool, double, std::vector<std::map<std::string, std::string>>> evaluate(
   const plansys2_msgs::msg::Tree & tree,
-  std::vector<plansys2::Predicate> & predicates,
+  std::unordered_set<plansys2::Instance> & instances,
+  std::unordered_set<plansys2::Predicate> & predicates,
   std::vector<plansys2::Function> & functions,
   bool apply = false,
   uint32_t node_id = 0);
@@ -82,7 +113,8 @@ bool check(
 
 bool check(
   const plansys2_msgs::msg::Tree & tree,
-  std::vector<plansys2::Predicate> & predicates,
+  std::unordered_set<plansys2::Instance> & instances,
+  std::unordered_set<plansys2::Predicate> & predicates,
   std::vector<plansys2::Function> & functions,
   uint32_t node_id = 0);
 
@@ -101,7 +133,8 @@ bool apply(
 
 bool apply(
   const plansys2_msgs::msg::Tree & tree,
-  std::vector<plansys2::Predicate> & predicates,
+  std::unordered_set<plansys2::Instance> & instances,
+  std::unordered_set<plansys2::Predicate> & predicates,
   std::vector<plansys2::Function> & functions,
   uint32_t node_id = 0);