Merge pull request #323 from Rezenders/action_execution

execute regular actions

plansys2_executor/include/plansys2_executor/ActionExecutor.hpp

@@ -19,9 +19,12 @@
 #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"
+#include "plansys2_msgs/msg/param.hpp"
+#include "plansys2_pddl_parser/Utils.h"
 #include "behaviortree_cpp/behavior_tree.h"
 
 #include "rclcpp/rclcpp.hpp"
@@ -104,14 +107,130 @@ class ActionExecutor
   rclcpp::TimerBase::SharedPtr waiting_timer_;
 };
 
+struct ActionVariant
+{
+  using shared_ptr_action = std::shared_ptr<plansys2_msgs::msg::Action>;
+  using shared_ptr_durative = std::shared_ptr<plansys2_msgs::msg::DurativeAction>;
+
+  std::variant<
+    std::shared_ptr<plansys2_msgs::msg::Action>,
+    std::shared_ptr<plansys2_msgs::msg::DurativeAction>> action;
+
+  ActionVariant & operator=(shared_ptr_action ptr)
+  {
+    action = ptr;
+    return *this;
+  }
+
+  ActionVariant & operator=(shared_ptr_durative ptr)
+  {
+    action = ptr;
+    return *this;
+  }
+
+  std::string get_action_string() const
+  {
+    std::string action_string;
+    if (std::holds_alternative<shared_ptr_action>(action)) {
+      action_string = parser::pddl::nameActionsToString(
+        std::get<shared_ptr_action>(action));
+    } else if (std::holds_alternative<shared_ptr_durative>(action)) {
+      action_string = parser::pddl::nameActionsToString(
+        std::get<shared_ptr_durative>(action));
+    }
+    return action_string;
+  }
+
+  std::string get_action_name() const
+  {
+    std::string action_name;
+    if (std::holds_alternative<shared_ptr_action>(action)) {
+      action_name = std::get<shared_ptr_action>(action)->name;
+    } else if (std::holds_alternative<shared_ptr_durative>(action)) {
+      action_name = std::get<shared_ptr_durative>(action)->name;
+    }
+    return action_name;
+  }
+
+  std::vector<plansys2_msgs::msg::Param> get_action_params() const
+  {
+    std::vector<plansys2_msgs::msg::Param> params;
+    if (std::holds_alternative<shared_ptr_action>(action)) {
+      params = std::get<shared_ptr_action>(action)->parameters;
+    } else if (std::holds_alternative<shared_ptr_durative>(action)) {
+      params = std::get<shared_ptr_durative>(action)->parameters;
+    }
+    return params;
+  }
+
+  plansys2_msgs::msg::Tree get_overall_requirements() const
+  {
+    plansys2_msgs::msg::Tree reqs;
+    if (std::holds_alternative<shared_ptr_action>(action)) {
+      reqs = std::get<shared_ptr_action>(action)->preconditions;
+    } else if (std::holds_alternative<shared_ptr_durative>(action)) {
+      reqs = std::get<shared_ptr_durative>(action)->over_all_requirements;
+    }
+    return reqs;
+  }
+
+  plansys2_msgs::msg::Tree get_at_start_requirements() const
+  {
+    plansys2_msgs::msg::Tree reqs;
+    if (std::holds_alternative<shared_ptr_durative>(action)) {
+      reqs = std::get<shared_ptr_durative>(action)->at_start_requirements;
+    }
+    return reqs;
+  }
+
+  plansys2_msgs::msg::Tree get_at_end_requirements() const
+  {
+    plansys2_msgs::msg::Tree reqs;
+    if (std::holds_alternative<shared_ptr_durative>(action)) {
+      reqs = std::get<shared_ptr_durative>(action)->at_end_requirements;
+    }
+    return reqs;
+  }
+
+  plansys2_msgs::msg::Tree get_at_start_effects() const
+  {
+    plansys2_msgs::msg::Tree effects;
+    if (std::holds_alternative<shared_ptr_durative>(action)) {
+      effects = std::get<shared_ptr_durative>(action)->at_start_effects;
+    }
+    return effects;
+  }
+
+  plansys2_msgs::msg::Tree get_at_end_effects() const
+  {
+    plansys2_msgs::msg::Tree effects;
+    if (std::holds_alternative<shared_ptr_action>(action)) {
+      effects = std::get<shared_ptr_action>(action)->effects;
+    } else if (std::holds_alternative<shared_ptr_durative>(action)) {
+      effects = std::get<shared_ptr_durative>(action)->at_end_effects;
+    }
+    return effects;
+  }
+
+  bool is_action() const
+  {
+    return std::holds_alternative<shared_ptr_action>(action);
+  }
+
+  bool is_durative_action() const
+  {
+    return std::holds_alternative<shared_ptr_durative>(action);
+  }
+};
+
 struct ActionExecutionInfo
 {
   std::shared_ptr<ActionExecutor> action_executor = {nullptr};
   bool at_start_effects_applied = {false};
   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};
+  ActionVariant action_info;
   std::string execution_error_info;
   double duration;
   double duration_overrun_percentage = -1.0;

plansys2_executor/include/plansys2_executor/BTBuilder.hpp

@@ -24,6 +24,7 @@
 
 #include "plansys2_executor/ActionExecutor.hpp"
 #include "plansys2_msgs/msg/plan.hpp"
+#include "plansys2_pddl_parser/Utils.h"
 
 namespace plansys2
 {
@@ -45,7 +46,7 @@ struct ActionStamped
   std::string expression;
   float duration;
   ActionType type;
-  std::shared_ptr<plansys2_msgs::msg::DurativeAction> action;
+  ActionVariant action;
 
   ActionStamped()
   : time(0.0), duration(0.0) {}

plansys2_executor/src/plansys2_executor/ComputeBT.cpp

@@ -257,15 +257,23 @@ ComputeBT::computeBTCallback(
   for (const auto & plan_item : plan.value().items) {
     auto index = BTBuilder::to_action_id(plan_item, 3);
 
+
     (*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));
+
+    auto actions = domain_client_->getActions();
+    std::string action_name_ = get_action_name(plan_item.action);
+    if (std::find(actions.begin(), actions.end(), action_name_) != actions.end()) {
+      (*action_map)[index].action_info.action = domain_client_->getAction(
+        action_name_, get_action_params(plan_item.action));
+    } else {
+      (*action_map)[index].action_info.action = domain_client_->getDurativeAction(
+        action_name_, get_action_params(plan_item.action));
+    }
 
     (*action_map)[index].duration = plan_item.duration;
-    std::string action_name = (*action_map)[index].durative_action_info->name;
+    std::string action_name = (*action_map)[index].action_info.get_action_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,21 @@ 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);
+    auto actions = domain_client_->getActions();
+    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 =
+        domain_client_->getAction(
+        action_name, get_action_params(plan_item.action));
+      apply(action->effects, local_predicates, local_functions);
+    } else {
+      std::shared_ptr<plansys2_msgs::msg::DurativeAction> action =
+        domain_client_->getDurativeAction(
+        action_name, get_action_params(plan_item.action));
+      apply(action->at_start_effects, local_predicates, local_functions);
+      apply(action->at_end_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 +378,23 @@ 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);
 
+
     (*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));
 
+    auto actions = domain_client_->getActions();
+    std::string action_name_ = get_action_name(plan_item.action);
+    if (std::find(actions.begin(), actions.end(), action_name_) != actions.end()) {
+      (*action_map)[index].action_info = domain_client_->getAction(
+        action_name_, get_action_params(plan_item.action));
+    } else {
+      (*action_map)[index].action_info = domain_client_->getDurativeAction(
+        action_name_, get_action_params(plan_item.action));
+    }
+
+    std::string action_name = (*action_map)[index].action_info.get_action_name();
     (*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 +636,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.action.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,6 +17,7 @@
 #include <memory>
 
 #include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp"
+#include "plansys2_msgs/msg/tree.hpp"
 
 namespace plansys2
 {
@@ -41,7 +42,7 @@ ApplyAtEndEffect::tick()
   std::string action;
   getInput("action", action);
 
-  auto effect = (*action_map_)[action].durative_action_info->at_end_effects;
+  auto effect = (*action_map_)[action].action_info.get_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

@@ -41,7 +41,11 @@ ApplyAtStartEffect::tick()
   std::string action;
   getInput("action", action);
 
-  auto effect = (*action_map_)[action].durative_action_info->at_start_effects;
+  if ((*action_map_)[action].action_info.is_action()) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  auto effect = (*action_map_)[action].action_info.get_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

@@ -44,15 +44,19 @@ CheckAtEndReq::tick()
 
   auto node = config().blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
 
-  auto reqs = (*action_map_)[action].durative_action_info->at_end_requirements;
+  if ((*action_map_)[action].action_info.is_action()) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  auto reqs = (*action_map_)[action].action_info.get_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,6 +18,7 @@
 #include <tuple>
 
 #include "plansys2_executor/behavior_tree/check_overall_req_node.hpp"
+#include "plansys2_msgs/msg/action.hpp"
 
 namespace plansys2
 {
@@ -44,15 +45,15 @@ CheckOverAllReq::tick()
 
   auto node = config().blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
 
-  auto reqs = (*action_map_)[action].durative_action_info->over_all_requirements;
+  auto reqs = (*action_map_)[action].action_info.get_overall_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,6 +18,7 @@
 #include <tuple>
 
 #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp"
+#include "plansys2_msgs/msg/tree.hpp"
 
 namespace plansys2
 {
@@ -44,8 +45,8 @@ 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;
+  auto reqs_as = (*action_map_)[action].action_info.get_at_start_requirements();
+  auto reqs_oa = (*action_map_)[action].action_info.get_overall_requirements();
 
   bool check_as = check(reqs_as, problem_client_);
   if (!check_as) {
@@ -54,7 +55,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 +67,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;
   }