execute regular actions

plansys2_executor/include/plansys2_executor/ActionExecutor.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <vector>
 
+#include "plansys2_msgs/msg/action.hpp"
 #include "plansys2_msgs/msg/action_execution.hpp"
 #include "plansys2_msgs/msg/action_execution_info.hpp"
 #include "plansys2_msgs/msg/durative_action.hpp"
@@ -111,7 +112,9 @@ struct ActionExecutionInfo
   bool at_end_effects_applied = {false};
   rclcpp::Time at_start_effects_applied_time;
   rclcpp::Time at_end_effects_applied_time;
-  std::shared_ptr<plansys2_msgs::msg::DurativeAction> durative_action_info = {nullptr};
+  std::variant<
+    std::shared_ptr<plansys2_msgs::msg::Action>,
+    std::shared_ptr<plansys2_msgs::msg::DurativeAction>> action_info;
   std::string execution_error_info;
   double duration;
   double duration_overrun_percentage = -1.0;

plansys2_executor/src/plansys2_executor/ComputeBT.cpp

@@ -257,15 +257,24 @@ ComputeBT::computeBTCallback(
   for (const auto & plan_item : plan.value().items) {
     auto index = BTBuilder::to_action_id(plan_item, 3);
 
+    std::string action_name;
     (*action_map)[index] = ActionExecutionInfo();
     (*action_map)[index].action_executor =
       ActionExecutor::make_shared(plan_item.action, shared_from_this());
-    (*action_map)[index].durative_action_info =
-      domain_client_->getDurativeAction(
-      get_action_name(plan_item.action), get_action_params(plan_item.action));
+    if (plan_item.duration > 0) {
+      auto action = domain_client_->getDurativeAction(
+        get_action_name(plan_item.action), get_action_params(plan_item.action));
+      action_name = action->name;
+      (*action_map)[index].action_info = action;
+    } else {
+      auto action = domain_client_->getAction(
+        get_action_name(plan_item.action), get_action_params(plan_item.action));
+      action_name = action->name;
+      (*action_map)[index].action_info = action;
+    }
 
     (*action_map)[index].duration = plan_item.duration;
-    std::string action_name = (*action_map)[index].durative_action_info->name;
+
     if (std::find(
         action_timeout_actions.begin(), action_timeout_actions.end(),
         action_name) != action_timeout_actions.end() &&

plansys2_executor/src/plansys2_executor/ExecutorNode.cpp

@@ -274,11 +274,19 @@ ExecutorNode::getOrderedSubGoals()
   }
 
   for (const auto & plan_item : current_plan_.value().items) {
-    std::shared_ptr<plansys2_msgs::msg::DurativeAction> action =
-      domain_client_->getDurativeAction(
-      get_action_name(plan_item.action), get_action_params(plan_item.action));
-    apply(action->at_start_effects, local_predicates, local_functions);
-    apply(action->at_end_effects, local_predicates, local_functions);
+    if (plan_item.duration > 0) {
+      std::shared_ptr<plansys2_msgs::msg::DurativeAction> action =
+        domain_client_->getDurativeAction(
+        get_action_name(plan_item.action), get_action_params(plan_item.action));
+      apply(action->at_start_effects, local_predicates, local_functions);
+      apply(action->at_end_effects, local_predicates, local_functions);
+    } else {
+      std::shared_ptr<plansys2_msgs::msg::Action> action =
+        domain_client_->getAction(
+        get_action_name(plan_item.action), get_action_params(plan_item.action));
+      apply(action->effects, local_predicates, local_functions);
+    }
+
 
     for (auto it = unordered_subgoals.begin(); it != unordered_subgoals.end(); ) {
       if (check(goal, local_predicates, local_functions, *it)) {
@@ -368,15 +376,26 @@ ExecutorNode::execute(const std::shared_ptr<GoalHandleExecutePlan> goal_handle)
   for (const auto & plan_item : current_plan_.value().items) {
     auto index = BTBuilder::to_action_id(plan_item, 3);
 
+    std::string action_name;
     (*action_map)[index] = ActionExecutionInfo();
     (*action_map)[index].action_executor =
       ActionExecutor::make_shared(plan_item.action, shared_from_this());
-    (*action_map)[index].durative_action_info =
-      domain_client_->getDurativeAction(
-      get_action_name(plan_item.action), get_action_params(plan_item.action));
+
+    if (plan_item.duration > 0) {
+      auto action =
+        domain_client_->getDurativeAction(
+        get_action_name(plan_item.action), get_action_params(plan_item.action));
+      action_name = action->name;
+      (*action_map)[index].action_info = action;
+    } else {
+      auto action =
+        domain_client_->getAction(
+        get_action_name(plan_item.action), get_action_params(plan_item.action));
+      action_name = action->name;
+      (*action_map)[index].action_info = action;
+    }
 
     (*action_map)[index].duration = plan_item.duration;
-    std::string action_name = (*action_map)[index].durative_action_info->name;
     if (std::find(
         action_timeout_actions.begin(), action_timeout_actions.end(),
         action_name) != action_timeout_actions.end() &&
@@ -618,8 +637,8 @@ ExecutorNode::print_execution_info(
         fprintf(stderr, "\tFAILURE\n");
         break;
     }
-    if (action_info.second.durative_action_info == nullptr) {
-      fprintf(stderr, "\tWith no duration info\n");
+    if (action_info.second.action_info.index() == std::variant_npos) {
+      fprintf(stderr, "\tWith no action info\n");
     }
 
     if (action_info.second.at_start_effects_applied) {

plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp

@@ -17,10 +17,15 @@
 #include <memory>
 
 #include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp"
+#include "plansys2_msgs/msg/tree.hpp"
 
 namespace plansys2
 {
 
+using shared_ptr_action = std::shared_ptr<plansys2_msgs::msg::Action>;
+using shared_ptr_durative = std::shared_ptr<plansys2_msgs::msg::DurativeAction>;
+
+
 ApplyAtEndEffect::ApplyAtEndEffect(
   const std::string & xml_tag_name,
   const BT::NodeConfig & conf)
@@ -41,7 +46,14 @@ ApplyAtEndEffect::tick()
   std::string action;
   getInput("action", action);
 
-  auto effect = (*action_map_)[action].durative_action_info->at_end_effects;
+  plansys2_msgs::msg::Tree effect;
+  if (std::holds_alternative<shared_ptr_action>((*action_map_)[action].action_info)) {
+    effect = std::get<shared_ptr_action>(
+      (*action_map_)[action].action_info)->effects;
+  } else if (std::holds_alternative<shared_ptr_durative>((*action_map_)[action].action_info)) {
+    effect = std::get<shared_ptr_durative>(
+      (*action_map_)[action].action_info)->at_end_effects;
+  }
 
   if (!(*action_map_)[action].at_end_effects_applied) {
     (*action_map_)[action].at_end_effects_applied = true;

plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp

@@ -21,6 +21,8 @@
 namespace plansys2
 {
 
+using shared_ptr_action = std::shared_ptr<plansys2_msgs::msg::Action>;
+
 ApplyAtStartEffect::ApplyAtStartEffect(
   const std::string & xml_tag_name,
   const BT::NodeConfig & conf)
@@ -41,7 +43,12 @@ ApplyAtStartEffect::tick()
   std::string action;
   getInput("action", action);
 
-  auto effect = (*action_map_)[action].durative_action_info->at_start_effects;
+  if (std::holds_alternative<shared_ptr_action>((*action_map_)[action].action_info)) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  auto effect = std::get<std::shared_ptr<plansys2_msgs::msg::DurativeAction>>(
+    (*action_map_)[action].action_info)->at_start_effects;
 
   if (!(*action_map_)[action].at_start_effects_applied) {
     (*action_map_)[action].at_start_effects_applied = true;

plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp

@@ -22,6 +22,8 @@
 namespace plansys2
 {
 
+using shared_ptr_action = std::shared_ptr<plansys2_msgs::msg::Action>;
+
 CheckAtEndReq::CheckAtEndReq(
   const std::string & xml_tag_name,
   const BT::NodeConfig & conf)
@@ -44,15 +46,20 @@ CheckAtEndReq::tick()
 
   auto node = config().blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
 
-  auto reqs = (*action_map_)[action].durative_action_info->at_end_requirements;
+  if (std::holds_alternative<shared_ptr_action>((*action_map_)[action].action_info)) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  auto reqs = std::get<std::shared_ptr<plansys2_msgs::msg::DurativeAction>>(
+    (*action_map_)[action].action_info)->at_end_requirements;
 
   if (!check(reqs, problem_client_)) {
     (*action_map_)[action].execution_error_info = "Error checking at end requirements";
 
     RCLCPP_ERROR_STREAM(
       node->get_logger(),
       "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " <<
-        parser::pddl::toString((*action_map_)[action].durative_action_info->at_end_requirements));
+        parser::pddl::toString(reqs));
 
     return BT::NodeStatus::FAILURE;
   } else {

plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp

@@ -18,10 +18,14 @@
 #include <tuple>
 
 #include "plansys2_executor/behavior_tree/check_overall_req_node.hpp"
+#include "plansys2_msgs/msg/action.hpp"
 
 namespace plansys2
 {
 
+using shared_ptr_action = std::shared_ptr<plansys2_msgs::msg::Action>;
+using shared_ptr_durative = std::shared_ptr<plansys2_msgs::msg::DurativeAction>;
+
 CheckOverAllReq::CheckOverAllReq(
   const std::string & xml_tag_name,
   const BT::NodeConfig & conf)
@@ -44,15 +48,22 @@ CheckOverAllReq::tick()
 
   auto node = config().blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
 
-  auto reqs = (*action_map_)[action].durative_action_info->over_all_requirements;
+  plansys2_msgs::msg::Tree reqs;
+  if (std::holds_alternative<shared_ptr_action>((*action_map_)[action].action_info)) {
+    reqs = std::get<shared_ptr_action>(
+      (*action_map_)[action].action_info)->preconditions;
+  } else if (std::holds_alternative<shared_ptr_durative>((*action_map_)[action].action_info)) {
+    reqs = std::get<shared_ptr_durative>(
+      (*action_map_)[action].action_info)->over_all_requirements;
+  }
 
   if (!check(reqs, problem_client_)) {
     (*action_map_)[action].execution_error_info = "Error checking over all requirements";
 
     RCLCPP_ERROR_STREAM(
       node->get_logger(),
       "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " <<
-        parser::pddl::toString((*action_map_)[action].durative_action_info->over_all_requirements));
+        parser::pddl::toString(reqs));
 
     return BT::NodeStatus::FAILURE;
   } else {

plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp

@@ -18,10 +18,14 @@
 #include <tuple>
 
 #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp"
+#include "plansys2_msgs/msg/tree.hpp"
 
 namespace plansys2
 {
 
+using shared_ptr_action = std::shared_ptr<plansys2_msgs::msg::Action>;
+using shared_ptr_durative = std::shared_ptr<plansys2_msgs::msg::DurativeAction>;
+
 WaitAtStartReq::WaitAtStartReq(
   const std::string & xml_tag_name,
   const BT::NodeConfig & conf)
@@ -44,8 +48,19 @@ WaitAtStartReq::tick()
 
   auto node = config().blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
 
-  auto reqs_as = (*action_map_)[action].durative_action_info->at_start_requirements;
-  auto reqs_oa = (*action_map_)[action].durative_action_info->over_all_requirements;
+  plansys2_msgs::msg::Tree reqs_as;
+  plansys2_msgs::msg::Tree reqs_oa;
+  if (std::holds_alternative<shared_ptr_action>((*action_map_)[action].action_info)) {
+    reqs_as = std::get<shared_ptr_action>(
+      (*action_map_)[action].action_info)->preconditions;
+    reqs_oa = std::get<shared_ptr_action>(
+      (*action_map_)[action].action_info)->preconditions;
+  } else if (std::holds_alternative<shared_ptr_durative>((*action_map_)[action].action_info)) {
+    reqs_as = std::get<shared_ptr_durative>(
+      (*action_map_)[action].action_info)->at_start_requirements;
+    reqs_oa = std::get<shared_ptr_durative>(
+      (*action_map_)[action].action_info)->over_all_requirements;
+  }
 
   bool check_as = check(reqs_as, problem_client_);
   if (!check_as) {
@@ -54,7 +69,7 @@ WaitAtStartReq::tick()
     RCLCPP_ERROR_STREAM(
       node->get_logger(),
       "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " <<
-        parser::pddl::toString((*action_map_)[action].durative_action_info->at_start_requirements));
+        parser::pddl::toString(reqs_as));
 
     return BT::NodeStatus::RUNNING;
   }
@@ -66,7 +81,7 @@ WaitAtStartReq::tick()
     RCLCPP_ERROR_STREAM(
       node->get_logger(),
       "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " <<
-        parser::pddl::toString((*action_map_)[action].durative_action_info->over_all_requirements));
+        parser::pddl::toString(reqs_oa));
 
     return BT::NodeStatus::RUNNING;
   }

plansys2_executor/test/unit/bt_node_test.cpp

@@ -107,14 +107,14 @@ TEST(problem_expert, wait_overall_req_test)
 
   auto action_map = std::make_shared<std::map<std::string, plansys2::ActionExecutionInfo>>();
   (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo();
-  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info =
+  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info =
     domain_client->getDurativeAction(
     plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"),
     plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)"));
 
   ASSERT_NE(
-    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info,
-    nullptr);
+    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info.index(),
+    std::variant_npos);
 
   std::string bt_xml_tree =
     R"(
@@ -221,14 +221,14 @@ TEST(problem_expert, wait_atstart_req_test)
 
   auto action_map = std::make_shared<std::map<std::string, plansys2::ActionExecutionInfo>>();
   (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo();
-  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info =
+  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info =
     domain_client->getDurativeAction(
     plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"),
     plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)"));
 
   ASSERT_NE(
-    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info,
-    nullptr);
+    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info.index(),
+    std::variant_npos);
 
   std::string bt_xml_tree =
     R"(
@@ -337,14 +337,14 @@ TEST(problem_expert, wait_atend_req_test)
 
   auto action_map = std::make_shared<std::map<std::string, plansys2::ActionExecutionInfo>>();
   (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo();
-  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info =
+  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info =
     domain_client->getDurativeAction(
     plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"),
     plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)"));
 
   ASSERT_NE(
-    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info,
-    nullptr);
+    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info.index(),
+    std::variant_npos);
 
   std::string bt_xml_tree =
     R"(
@@ -451,14 +451,14 @@ TEST(problem_expert, at_start_effect_test)
 
   auto action_map = std::make_shared<std::map<std::string, plansys2::ActionExecutionInfo>>();
   (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo();
-  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info =
+  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info =
     domain_client->getDurativeAction(
     plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"),
     plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)"));
 
   ASSERT_NE(
-    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info,
-    nullptr);
+    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info.index(),
+    std::variant_npos);
 
   std::string bt_xml_tree =
     R"(
@@ -570,14 +570,14 @@ TEST(problem_expert, at_end_effect_test)
 
   auto action_map = std::make_shared<std::map<std::string, plansys2::ActionExecutionInfo>>();
   (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo();
-  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info =
+  (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info =
     domain_client->getDurativeAction(
     plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"),
     plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)"));
 
   ASSERT_NE(
-    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info,
-    nullptr);
+    (*action_map)["(move robot1 wheels_zone assembly_zone):5"].action_info.index(),
+    std::variant_npos);
 
   std::string bt_xml_tree =
     R"(