Implement BehaviourTrees for forall and imply

plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp

@@ -142,7 +142,8 @@ class DomainExpert : public DomainExpertInterface
    */
   plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction(
     const std::string & action,
-    const std::vector<std::string> & params = {});
+    const std::vector<std::string> & params = {},
+    const std::vector<plansys2_msgs::msg::Param> & instances = {});
 
   /// Get the current domain, ready to be saved to file, or to initialize another domain.
   /**

plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp

@@ -151,7 +151,8 @@ class DomainExpertClient : public DomainExpertInterface
    */
   plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction(
     const std::string & action,
-    const std::vector<std::string> & params = {});
+    const std::vector<std::string> & params = {},
+    const std::vector<plansys2_msgs::msg::Param> & instances = {});
 
   /// Get the current domain, ready to be saved to file, or to initialize another domain.
   /**

plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp

@@ -132,8 +132,8 @@ class DomainExpertInterface
    *    effects. If the action does not exist, the value returned has not value.
    */
   virtual plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction(
-    const std::string & durative_action, const std::vector<std::string> & params) =
-  0;
+    const std::string & durative_action, const std::vector<std::string> & params,
+    const std::vector<plansys2_msgs::msg::Param> & instances) = 0;
 
   /// Get the current domain, ready to be saved to file, or to initialize another domain.
   /**

plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp

@@ -303,7 +303,9 @@ DomainExpert::getDurativeActions()
 }
 
 plansys2_msgs::msg::DurativeAction::SharedPtr
-DomainExpert::getDurativeAction(const std::string & action, const std::vector<std::string> & params)
+DomainExpert::getDurativeAction(
+  const std::string & action, const std::vector<std::string> & params,
+  const std::vector<plansys2_msgs::msg::Param> & instances)
 {
   std::string action_search = action;
   std::transform(
@@ -335,20 +337,24 @@ DomainExpert::getDurativeAction(const std::string & action, const std::vector<st
         domain_->types[action_obj->params[j]]->getSubTypesNames(param.sub_types);
         ret->parameters.push_back(param);
       }
-
+      // Instances
+      std::map<std::string, std::vector<std::string>> instances_map;
+      for (const auto & instance : instances) {
+        instances_map[instance.type].push_back(instance.name);
+      }
       // Preconditions AtStart
       if (action_obj->pre) {
-        action_obj->pre->getTree(ret->at_start_requirements, *domain_, params);
+        action_obj->pre->getTree(ret->at_start_requirements, *domain_, params, instances_map);
       }
 
       // Preconditions OverAll
       if (action_obj->pre_o) {
-        action_obj->pre_o->getTree(ret->over_all_requirements, *domain_, params);
+        action_obj->pre_o->getTree(ret->over_all_requirements, *domain_, params, instances_map);
       }
 
       // Preconditions AtEnd
       if (action_obj->pre_e) {
-        action_obj->pre_e->getTree(ret->at_end_requirements, *domain_, params);
+        action_obj->pre_e->getTree(ret->at_end_requirements, *domain_, params, instances_map);
       }
 
       // Effects AtStart

plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp

@@ -493,7 +493,8 @@ DomainExpertClient::getDurativeActions()
 plansys2_msgs::msg::DurativeAction::SharedPtr
 DomainExpertClient::getDurativeAction(
   const std::string & action,
-  const std::vector<std::string> & params)
+  const std::vector<std::string> & params,
+  const std::vector<plansys2_msgs::msg::Param> & instances)
 {
   while (!get_durative_action_details_client_->wait_for_service(std::chrono::seconds(1))) {
     if (!rclcpp::ok()) {
@@ -509,6 +510,7 @@ DomainExpertClient::getDurativeAction(
 
   request->durative_action = action;
   request->parameters = params;
+  request->instances = instances;
 
   auto future_result = get_durative_action_details_client_->async_send_request(request);
 

plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp

@@ -337,7 +337,8 @@ DomainExpertNode::get_domain_durative_action_details_service_callback(
 
     RCLCPP_WARN(get_logger(), "Requesting service in non-active state");
   } else {
-    auto action = domain_expert_->getDurativeAction(request->durative_action, request->parameters);
+    auto action = domain_expert_->getDurativeAction(
+      request->durative_action, request->parameters, request->instances);
 
     if (action) {
       response->durative_action = *action;

plansys2_executor/src/plansys2_executor/ExecutorNode.cpp

@@ -268,6 +268,8 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info)
   auto goal = problem_client_->getGoal();
   auto local_predicates = problem_client_->getPredicates();
   auto local_functions = problem_client_->getFunctions();
+  auto instances = plansys2::convertVector<plansys2_msgs::msg::Param, plansys2::Instance>(
+    problem_client_->getInstances());
 
   std::vector<uint32_t> unordered_subgoals = parser::pddl::getSubtreeIds(goal);
 
@@ -285,6 +287,8 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info)
 
   for (const auto & plan_item : runtime_info.complete_plan.items) {
     auto actions = domain_client_->getActions();
+    auto instances = plansys2::convertVector<plansys2_msgs::msg::Param, plansys2::Instance>(
+      problem_client_->getInstances());
     std::string action_name = get_action_name(plan_item.action);
     if (std::find(actions.begin(), actions.end(), action_name) != actions.end()) {
       std::shared_ptr<plansys2_msgs::msg::Action> action =
@@ -294,7 +298,7 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info)
     } else {
       std::shared_ptr<plansys2_msgs::msg::DurativeAction> action =
         domain_client_->getDurativeAction(
-        action_name, get_action_params(plan_item.action));
+        action_name, get_action_params(plan_item.action), instances);
       apply(action->at_start_effects, local_predicates, local_functions);
       apply(action->at_end_effects, local_predicates, local_functions);
     }
@@ -347,7 +351,8 @@ ExecutorNode::create_plan_runtime_info(PlanRuntineInfo & runtime_info)
 {
   runtime_info.action_map = std::make_shared<std::map<std::string, ActionExecutionInfo>>();
   auto action_timeout_actions = this->get_parameter("action_timeouts.actions").as_string_array();
-
+  auto instances = plansys2::convertVector<plansys2_msgs::msg::Param, plansys2::Instance>(
+    problem_client_->getInstances());
   (*runtime_info.action_map)[":0"] = ActionExecutionInfo();
   (*runtime_info.action_map)[":0"].action_executor = ActionExecutor::make_shared("(INIT)",
     shared_from_this());
@@ -372,7 +377,7 @@ ExecutorNode::create_plan_runtime_info(PlanRuntineInfo & runtime_info)
         action_name, get_action_params(plan_item.action));
     } else {
       (*runtime_info.action_map)[index].action_info = domain_client_->getDurativeAction(
-        action_name, get_action_params(plan_item.action));
+        action_name, get_action_params(plan_item.action), instances);
     }
 
     action_name = (*runtime_info.action_map)[index].action_info.get_action_name();

plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp

@@ -824,7 +824,8 @@ std::vector<ActionStamped>
 SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan)
 {
   std::vector<ActionStamped> ret;
-
+  auto instances = plansys2::convertVector<plansys2_msgs::msg::Param, plansys2::Instance>(
+    problem_client_->getInstances());
   for (auto & item : plan.items) {
     ActionStamped action_stamped;
 
@@ -838,7 +839,7 @@ SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan)
     } else {
       action_stamped.action.action =
         domain_client_->getDurativeAction(
-        get_action_name(item.action), get_action_params(item.action));
+        get_action_name(item.action), get_action_params(item.action), instances);
     }
 
     ret.push_back(action_stamped);

plansys2_executor/test/pddl/test_forall_imply.pddl

@@ -0,0 +1,27 @@
+(define (domain test_forall_imply)
+  (:requirements :strips :adl :typing :durative-actions :fluents)
+  (:types
+    object
+    item - object
+    location - object
+  )
+  (:predicates
+    (item_at ?item - item ?location - location)
+    (item_checked ?item - item)
+    (all_items_checked ?location - location)
+  )
+  (:durative-action check_items
+    :parameters (?location - location)
+    :duration (= ?duration 1)
+    :condition
+      (and (over all 
+        (forall (?item - item)
+          (imply (item_at ?item ?location) (item_checked ?item))
+        )
+      ))
+    :effect
+      (and
+        (at end (all_items_checked ?location))
+      )
+  )
+)

plansys2_executor/test/unit/bt_node_test.cpp

@@ -761,6 +761,117 @@ TEST(problem_expert, at_end_effect_test)
   t.join();
 }
 
+TEST(problem_expert, forall_imply_test)
+{
+  auto test_node = rclcpp::Node::make_shared("test_node");
+  auto test_lc_node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node");
+  auto domain_node = std::make_shared<plansys2::DomainExpertNode>();
+  auto problem_node = std::make_shared<plansys2::ProblemExpertNode>();
+
+  auto domain_client = std::make_shared<plansys2::DomainExpertClient>();
+  auto problem_client = std::make_shared<plansys2::ProblemExpertClient>();
+
+  std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_executor");
+
+  domain_node->set_parameter({"model_file", pkgpath + "/pddl/test_forall_imply.pddl"});
+  problem_node->set_parameter({"model_file", pkgpath + "/pddl/test_forall_imply.pddl"});
+
+  rclcpp::executors::MultiThreadedExecutor exe(rclcpp::ExecutorOptions(), 8);
+
+  exe.add_node(domain_node->get_node_base_interface());
+  exe.add_node(problem_node->get_node_base_interface());
+
+  bool finish = false;
+  std::thread t([&]() {
+      while (!finish) {exe.spin_some();}
+    });
+
+  domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+
+  {
+    rclcpp::Rate rate(10);
+    auto start = test_node->now();
+    while ((test_node->now() - start).seconds() < 0.5) {
+      rate.sleep();
+    }
+  }
+
+  domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+  problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
+  {
+    rclcpp::Rate rate(10);
+    auto start = test_node->now();
+    while ((test_node->now() - start).seconds() < 0.5) {
+      rate.sleep();
+    }
+  }
+
+  ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("location1", "location")));
+  ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("item1", "item")));
+  ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("item2", "item")));
+
+  auto instances = plansys2::convertVector<plansys2_msgs::msg::Param, plansys2::Instance>(
+    problem_client->getInstances());
+  auto action_map = std::make_shared<std::map<std::string, plansys2::ActionExecutionInfo>>();
+  (*action_map)["(check_items location1):1"] = plansys2::ActionExecutionInfo();
+  (*action_map)["(check_items location1):1"].action_info =
+    domain_client->getDurativeAction(
+    plansys2::get_action_name("(check_items location1)"),
+    plansys2::get_action_params("(check_items location1)"), instances);
+
+  ASSERT_NE(
+    (*action_map)["(check_items location1):1"].action_info.action.index(),
+    std::variant_npos);
+
+  std::string bt_xml_tree =
+    R"(
+    <root main_tree_to_execute = "MainTree" >
+      <BehaviorTree ID="MainTree">
+        <Sequence name="root_sequence">
+          <CheckOverAllReq action="(check_items location1):1"/>
+       </Sequence>
+      </BehaviorTree>
+    </root>
+  )";
+
+  auto blackboard = BT::Blackboard::create();
+
+  blackboard->set("action_map", action_map);
+  blackboard->set("node", test_lc_node);
+  blackboard->set("problem_client", problem_client);
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<plansys2::ExecuteAction>("ExecuteAction");
+  factory.registerNodeType<plansys2::CheckOverAllReq>("CheckOverAllReq");
+
+
+  std::vector<std::string> predicates = {
+    "(item_at item1 location1)",
+    "(item_at item2 location1)",
+    "(item_checked item1)",
+    "(item_checked item2)"
+  };
+
+  try {
+    for (const auto & pred : predicates) {
+      ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred)));
+    }
+
+    auto tree = factory.createTreeFromText(bt_xml_tree, blackboard);
+
+    auto status = BT::NodeStatus::RUNNING;
+    status = tree.tickOnce();
+    ASSERT_EQ(status, BT::NodeStatus::SUCCESS);
+  } catch (std::exception & e) {
+    std::cerr << e.what() << std::endl;
+  }
+
+  finish = true;
+  t.join();
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);

plansys2_msgs/msg/Node.msg

@@ -14,6 +14,8 @@ uint8 NUMBER = 9
 uint8 CONSTANT = 10
 uint8 PARAMETER = 11
 uint8 EXISTS = 12
+uint8 FORALL = 13
+uint8 IMPLY = 14
 
 # Expression types
 uint8 COMP_EQ = 13

plansys2_msgs/srv/GetDomainDurativeActionDetails.srv

@@ -1,5 +1,6 @@
 string durative_action
 string[] parameters
+plansys2_msgs/Param[] instances
 ---
 bool success
 plansys2_msgs/DurativeAction durative_action

plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp

@@ -16,6 +16,7 @@
 #include <algorithm>
 #include <string>
 #include <vector>
+#include <map>
 
 #include "plansys2_msgs/msg/node.hpp"
 #include "plansys2_msgs/msg/tree.hpp"
@@ -65,7 +66,8 @@ class Action : public ParamCond
 
   plansys2_msgs::msg::Node::SharedPtr getTree(
     plansys2_msgs::msg::Tree & tree, const Domain & d,
-    const std::vector<std::string> & replace = {}) const override;
+    const std::vector<std::string> & replace = {},
+    const std::map<std::string, std::vector<std::string>> & instances_map = {}) const override;
 
   void parseConditions(Stringreader & f, TokenStruct<std::string> & ts, Domain & d);
 

plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp

@@ -16,6 +16,7 @@
 #include <algorithm>
 #include <string>
 #include <vector>
+#include <map>
 
 #include "plansys2_msgs/msg/node.hpp"
 #include "plansys2_msgs/msg/tree.hpp"
@@ -54,7 +55,8 @@ class And : public Condition
 
   plansys2_msgs::msg::Node::SharedPtr getTree(
     plansys2_msgs::msg::Tree & tree, const Domain & d,
-    const std::vector<std::string> & replace = {}) const override;
+    const std::vector<std::string> & replace = {},
+    const std::map<std::string, std::vector<std::string>> & instances_map = {}) const override;
 
   void parse(Stringreader & f, TokenStruct<std::string> & ts, Domain & d);
 

plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp

@@ -17,6 +17,7 @@
 #include <stdexcept>
 #include <string>
 #include <vector>
+#include <map>
 
 #include "plansys2_msgs/msg/node.hpp"
 #include "plansys2_msgs/msg/tree.hpp"
@@ -52,7 +53,8 @@ class Condition
 
   virtual plansys2_msgs::msg::Node::SharedPtr getTree(
     plansys2_msgs::msg::Tree & tree, const Domain & d,
-    const std::vector<std::string> & replace = {}) const = 0;
+    const std::vector<std::string> & replace = {},
+    const std::map<std::string, std::vector<std::string>> & instances_map = {}) const = 0;
 
   virtual void parse(Stringreader & f, TokenStruct<std::string> & ts, Domain & d) = 0;
 

plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp

@@ -16,6 +16,7 @@
 #include <algorithm>
 #include <string>
 #include <vector>
+#include <map>
 
 #include "plansys2_msgs/msg/node.hpp"
 #include "plansys2_msgs/msg/tree.hpp"
@@ -53,7 +54,8 @@ class Derived : public Lifted
 
   plansys2_msgs::msg::Node::SharedPtr getTree(
     plansys2_msgs::msg::Tree & tree, const Domain & d,
-    const std::vector<std::string> & replace = {}) const override;
+    const std::vector<std::string> & replace = {},
+    const std::map<std::string, std::vector<std::string>> & instances_map = {}) const override;
 
   void parse(Stringreader & f, TokenStruct<std::string> & ts, Domain & d);