diff --git a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp index 912b472d..50529d46 100644 --- a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp +++ b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp @@ -19,9 +19,12 @@ #include #include +#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,6 +107,122 @@ class ActionExecutor rclcpp::TimerBase::SharedPtr waiting_timer_; }; +struct ActionVariant +{ + using shared_ptr_action = std::shared_ptr; + using shared_ptr_durative = std::shared_ptr; + + std::variant< + std::shared_ptr, + std::shared_ptr> 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(action)) { + action_string = parser::pddl::nameActionsToString( + std::get(action)); + } else if (std::holds_alternative(action)) { + action_string = parser::pddl::nameActionsToString( + std::get(action)); + } + return action_string; + } + + std::string get_action_name() const + { + std::string action_name; + if (std::holds_alternative(action)) { + action_name = std::get(action)->name; + } else if (std::holds_alternative(action)) { + action_name = std::get(action)->name; + } + return action_name; + } + + std::vector get_action_params() const + { + std::vector params; + if (std::holds_alternative(action)) { + params = std::get(action)->parameters; + } else if (std::holds_alternative(action)) { + params = std::get(action)->parameters; + } + return params; + } + + plansys2_msgs::msg::Tree get_overall_requirements() const + { + plansys2_msgs::msg::Tree reqs; + if (std::holds_alternative(action)) { + reqs = std::get(action)->preconditions; + } else if (std::holds_alternative(action)) { + reqs = std::get(action)->over_all_requirements; + } + return reqs; + } + + plansys2_msgs::msg::Tree get_at_start_requirements() const + { + plansys2_msgs::msg::Tree reqs; + if (std::holds_alternative(action)) { + reqs = std::get(action)->at_start_requirements; + } + return reqs; + } + + plansys2_msgs::msg::Tree get_at_end_requirements() const + { + plansys2_msgs::msg::Tree reqs; + if (std::holds_alternative(action)) { + reqs = std::get(action)->at_end_requirements; + } + return reqs; + } + + plansys2_msgs::msg::Tree get_at_start_effects() const + { + plansys2_msgs::msg::Tree effects; + if (std::holds_alternative(action)) { + effects = std::get(action)->at_start_effects; + } + return effects; + } + + plansys2_msgs::msg::Tree get_at_end_effects() const + { + plansys2_msgs::msg::Tree effects; + if (std::holds_alternative(action)) { + effects = std::get(action)->effects; + } else if (std::holds_alternative(action)) { + effects = std::get(action)->at_end_effects; + } + return effects; + } + + bool is_action() const + { + return std::holds_alternative(action); + } + + bool is_durative_action() const + { + return std::holds_alternative(action); + } +}; + struct ActionExecutionInfo { std::shared_ptr action_executor = {nullptr}; @@ -111,7 +230,7 @@ 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 durative_action_info = {nullptr}; + ActionVariant action_info; std::string execution_error_info; double duration; double duration_overrun_percentage = -1.0; diff --git a/plansys2_executor/include/plansys2_executor/BTBuilder.hpp b/plansys2_executor/include/plansys2_executor/BTBuilder.hpp index e869dbff..25ba731f 100644 --- a/plansys2_executor/include/plansys2_executor/BTBuilder.hpp +++ b/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 action; + ActionVariant action; ActionStamped() : time(0.0), duration(0.0) {} diff --git a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp index 27e05150..e8bed028 100644 --- a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp +++ b/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() && diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index f444863d..743dee1c 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/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 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 action = + domain_client_->getAction( + action_name, get_action_params(plan_item.action)); + apply(action->effects, local_predicates, local_functions); + } else { + std::shared_ptr 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 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) { diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp index 6cb2e59d..80e99b0f 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp @@ -17,6 +17,7 @@ #include #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; diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp index 4e57977f..a18a10f9 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp +++ b/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; diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp index df013112..c96662c4 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp @@ -44,7 +44,11 @@ CheckAtEndReq::tick() auto node = config().blackboard->get("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"; @@ -52,7 +56,7 @@ CheckAtEndReq::tick() 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 { diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp index 227ac6dc..507a688b 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp @@ -18,6 +18,7 @@ #include #include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" +#include "plansys2_msgs/msg/action.hpp" namespace plansys2 { @@ -44,7 +45,7 @@ CheckOverAllReq::tick() auto node = config().blackboard->get("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"; @@ -52,7 +53,7 @@ CheckOverAllReq::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)); return BT::NodeStatus::FAILURE; } else { diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp index 50c55f03..2e2c6ce6 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp @@ -18,6 +18,7 @@ #include #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("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; } diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp index 058fd1ba..e7ae0bb6 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp @@ -70,9 +70,13 @@ SimpleBTBuilder::is_action_executable( std::vector & predicates, std::vector & functions) const { - return check(action.action->at_start_requirements, predicates, functions) && - check(action.action->at_end_requirements, predicates, functions) && - check(action.action->over_all_requirements, predicates, functions); + if (action.action.is_action()) { + return check(action.action.get_overall_requirements(), predicates, functions); + } + + return check(action.action.get_at_start_requirements(), predicates, functions) && + check(action.action.get_at_end_requirements(), predicates, functions) && + check(action.action.get_overall_requirements(), predicates, functions); } ActionNode::Ptr @@ -95,8 +99,10 @@ SimpleBTBuilder::get_node_satisfy( bool satisfied_before = check(requirement, predicates, functions); // Apply the effects - apply(node->action.action->at_start_effects, predicates, functions); - apply(node->action.action->at_end_effects, predicates, functions); + if (node->action.action.is_durative_action()) { + apply(node->action.action.get_at_start_effects(), predicates, functions); + } + apply(node->action.action.get_at_end_effects(), predicates, functions); // Is the requirement satisfied after applying the effects? bool satisfied_after = check(requirement, predicates, functions); @@ -134,7 +140,9 @@ SimpleBTBuilder::get_node_contradict( // Are all of the requirements satisfied? if (is_action_executable(current->action, predicates, functions)) { // Apply the effects - apply(current->action.action->at_start_effects, predicates, functions); + if (current->action.action.is_durative_action()) { + apply(current->action.action.get_at_start_effects(), predicates, functions); + } // Look for a contradiction if (!is_action_executable(node->action, predicates, functions)) { @@ -158,7 +166,9 @@ SimpleBTBuilder::is_parallelizable( // Apply the "at start" effects of the new action. auto preds = predicates; auto funcs = functions; - apply(action.action->at_start_effects, preds, funcs); + if (action.action.is_durative_action()) { + apply(action.action.get_at_start_effects(), preds, funcs); + } // Check the requirements of the actions in the input set. for (const auto & other : nodes) { @@ -172,7 +182,10 @@ SimpleBTBuilder::is_parallelizable( // Apply the "at start" effects of the action. preds = predicates; funcs = functions; - apply(other->action.action->at_start_effects, preds, funcs); + + if (other->action.action.is_durative_action()) { + apply(other->action.action.get_at_start_effects(), preds, funcs); + } // Check the requirements of the new action. if (!is_action_executable(action, preds, funcs)) { @@ -308,8 +321,10 @@ SimpleBTBuilder::get_state( for (auto & in : node->in_arcs) { if (std::find(used_nodes.begin(), used_nodes.end(), in) == used_nodes.end()) { get_state(in, used_nodes, predicates, functions); - apply(in->action.action->at_start_effects, predicates, functions); - apply(in->action.action->at_end_effects, predicates, functions); + if (in->action.action.is_durative_action()) { + apply(in->action.action.get_at_start_effects(), predicates, functions); + } + apply(in->action.action.get_at_end_effects(), predicates, functions); used_nodes.push_back(in); } } @@ -347,12 +362,12 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) } new_node->level_num = level_counter; - std::vector at_start_requirements = - parser::pddl::getSubtrees(new_node->action.action->at_start_requirements); - std::vector over_all_requirements = - parser::pddl::getSubtrees(new_node->action.action->over_all_requirements); - std::vector at_end_requirements = - parser::pddl::getSubtrees(new_node->action.action->at_end_requirements); + auto over_all_requirements = parser::pddl::getSubtrees( + new_node->action.action.get_overall_requirements()); + auto at_start_requirements = parser::pddl::getSubtrees( + new_node->action.action.get_at_start_requirements()); + auto at_end_requirements = parser::pddl::getSubtrees( + new_node->action.action.get_at_end_requirements()); std::vector requirements; requirements.insert( @@ -597,8 +612,7 @@ SimpleBTBuilder::get_flow_tree( std::string ret; int l = level; - const std::string action_id = "(" + parser::pddl::nameActionsToString(node->action.action) + - "):" + + const std::string action_id = "(" + node->action.action.get_action_string() + "):" + std::to_string(static_cast(node->action.time * 1000)); if (std::find(used_nodes.begin(), used_nodes.end(), action_id) != used_nodes.end()) { @@ -657,7 +671,7 @@ SimpleBTBuilder::get_node_dotgraph( { std::stringstream ss; ss << t(level); - ss << node->node_num << " [label=\"" << parser::pddl::nameActionsToString(node->action.action) << + ss << node->node_num << " [label=\"" << node->action.action.get_action_string() << "\""; ss << "labeljust=c,style=filled"; @@ -689,7 +703,7 @@ ActionExecutor::Status SimpleBTBuilder::get_action_status( ActionStamped action_stamped, std::shared_ptr> action_map) { - auto index = "(" + parser::pddl::nameActionsToString(action_stamped.action) + "):" + + auto index = "(" + action_stamped.action.get_action_string() + "):" + std::to_string(static_cast(action_stamped.time * 1000)); if ((*action_map)[index].action_executor) { return (*action_map)[index].action_executor->get_internal_status(); @@ -777,13 +791,14 @@ SimpleBTBuilder::execution_block(const ActionNode::Ptr & node, int l) const auto & action = node->action; std::string ret; std::string ret_aux = bt_action_; - const std::string action_id = "(" + parser::pddl::nameActionsToString(action.action) + "):" + + + const std::string action_id = "(" + node->action.action.get_action_string() + "):" + std::to_string(static_cast(action.time * 1000)); std::string wait_actions; for (const auto & previous_node : node->in_arcs) { const std::string parent_action_id = "(" + - parser::pddl::nameActionsToString(previous_node->action.action) + "):" + + previous_node->action.action.get_action_string() + "):" + std::to_string(static_cast( previous_node->action.time * 1000)); wait_actions = wait_actions + t(1) + ""; @@ -815,9 +830,16 @@ SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) action_stamped.time = item.time; action_stamped.duration = item.duration; - action_stamped.action = - domain_client_->getDurativeAction( - get_action_name(item.action), get_action_params(item.action)); + auto actions = domain_client_->getActions(); + if (std::find(actions.begin(), actions.end(), get_action_name(item.action)) != actions.end()) { + action_stamped.action.action = + domain_client_->getAction( + get_action_name(item.action), get_action_params(item.action)); + } else { + action_stamped.action.action = + domain_client_->getDurativeAction( + get_action_name(item.action), get_action_params(item.action)); + } ret.push_back(action_stamped); } @@ -832,8 +854,8 @@ SimpleBTBuilder::print_node( std::set & used_nodes) const { std::cerr << std::string(level, '\t') << "[" << node->action.time << "] "; - std::cerr << node->action.action->name << " "; - for (const auto & param : node->action.action->parameters) { + std::cerr << node->action.action.get_action_name() << " "; + for (const auto & param : node->action.action.get_action_params()) { std::cerr << param.name << " "; } std::cerr << " in arcs " << node->in_arcs.size() << " "; @@ -859,9 +881,9 @@ SimpleBTBuilder::print_node_csv(const plansys2::ActionNode::Ptr & node, uint32_t std::string out_str = std::to_string(root_num) + ", " + std::to_string(node->node_num) + ", " + std::to_string(node->level_num) + ", " + - parser::pddl::nameActionsToString(node->action.action); + node->action.action.get_action_string(); for (const auto & arc : node->out_arcs) { - out_str = out_str + ", " + parser::pddl::nameActionsToString(arc->action.action); + out_str = out_str + ", " + arc->action.action.get_action_string(); } std::cerr << out_str << std::endl; for (const auto & out : node->out_arcs) { @@ -888,7 +910,7 @@ SimpleBTBuilder::get_node_tabular( graph.push_back( std::make_tuple( root_num, node->node_num, node->level_num, - parser::pddl::nameActionsToString(node->action.action))); + node->action.action.get_action_string())); for (const auto & out : node->out_arcs) { get_node_tabular(out, root_num, graph); } diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp index e32f4b3a..912a0ba1 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp @@ -263,10 +263,12 @@ STNBTBuilder::init_graph(const plansys2_msgs::msg::Plan & plan) const auto predicates = problem_client_->getPredicates(); auto functions = problem_client_->getFunctions(); + auto init_action = std::make_shared(); + init_action->at_end_effects = from_state(predicates, functions); + int node_cnt = 0; auto init_node = Node::make_shared(node_cnt++); - init_node->action.action = std::make_shared(); - init_node->action.action->at_end_effects = from_state(predicates, functions); + init_node->action.action = init_action; init_node->action.type = ActionType::INIT; graph->nodes.push_back(init_node); @@ -291,9 +293,10 @@ STNBTBuilder::init_graph(const plansys2_msgs::msg::Plan & plan) const auto goal = problem_client_->getGoal(); plansys2_msgs::msg::Tree * goal_tree = &goal; + auto goal_action = std::make_shared(); + goal_action->at_start_requirements = *goal_tree; auto goal_node = Node::make_shared(node_cnt++); - goal_node->action.action = std::make_shared(); - goal_node->action.action->at_start_requirements = *goal_tree; + goal_node->action.action = goal_action; goal_node->action.type = ActionType::GOAL; graph->nodes.push_back(goal_node); @@ -311,9 +314,16 @@ STNBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) const action_stamped.expression = item.action; action_stamped.duration = item.duration; action_stamped.type = ActionType::DURATIVE; - action_stamped.action = - domain_client_->getDurativeAction( - get_action_name(item.action), get_action_params(item.action)); + auto actions = domain_client_->getActions(); + if (std::find(actions.begin(), actions.end(), get_action_name(item.action)) != actions.end()) { + action_stamped.action = + domain_client_->getAction( + get_action_name(item.action), get_action_params(item.action)); + } else { + action_stamped.action = + domain_client_->getDurativeAction( + get_action_name(item.action), get_action_params(item.action)); + } ret.push_back(action_stamped); } @@ -384,9 +394,10 @@ STNBTBuilder::get_simple_plan(const plansys2_msgs::msg::Plan & plan) const auto predicates = problem_client_->getPredicates(); auto functions = problem_client_->getFunctions(); + auto init_action_ = std::make_shared(); + init_action_->at_end_effects = from_state(predicates, functions); ActionStamped init_action; - init_action.action = std::make_shared(); - init_action.action->at_end_effects = from_state(predicates, functions); + init_action.action = init_action_; init_action.type = ActionType::INIT; simple_plan.insert(std::make_pair(-1, init_action)); @@ -406,9 +417,11 @@ STNBTBuilder::get_simple_plan(const plansys2_msgs::msg::Plan & plan) const auto goal = problem_client_->getGoal(); plansys2_msgs::msg::Tree * goal_tree = &goal; + auto goal_action_ = std::make_shared(); + goal_action_->at_start_requirements = *goal_tree; + ActionStamped goal_action; - goal_action.action = std::make_shared(); - goal_action.action->at_start_requirements = *goal_tree; + goal_action.action = goal_action_; goal_action.type = ActionType::GOAL; simple_plan.insert(std::make_pair(max_time, goal_action)); @@ -474,9 +487,11 @@ STNBTBuilder::get_states( auto it = plan.equal_range(time); for (auto iter = it.first; iter != it.second; ++iter) { if (iter->second.type == ActionType::START) { - apply(iter->second.action->at_start_effects, state_vec.predicates, state_vec.functions); + apply( + iter->second.action.get_at_start_effects(), state_vec.predicates, state_vec.functions); } else if (iter->second.type == ActionType::END) { - apply(iter->second.action->at_end_effects, state_vec.predicates, state_vec.functions); + apply( + iter->second.action.get_at_end_effects(), state_vec.predicates, state_vec.functions); } } states.insert(std::make_pair(time, state_vec)); @@ -927,11 +942,11 @@ plansys2_msgs::msg::Tree STNBTBuilder::get_conditions(const ActionStamped & action) const { if (action.type == ActionType::START || action.type == ActionType::GOAL) { - return action.action->at_start_requirements; + return action.action.get_at_start_requirements(); } else if (action.type == ActionType::OVERALL) { - return action.action->over_all_requirements; + return action.action.get_overall_requirements(); } else if (action.type == ActionType::END) { - return action.action->at_end_requirements; + return action.action.get_at_end_requirements(); } return plansys2_msgs::msg::Tree(); @@ -941,9 +956,9 @@ plansys2_msgs::msg::Tree STNBTBuilder::get_effects(const ActionStamped & action) const { if (action.type == ActionType::START) { - return action.action->at_start_effects; + return action.action.get_at_start_effects(); } else if (action.type == ActionType::END || action.type == ActionType::INIT) { - return action.action->at_end_effects; + return action.action.get_at_end_effects(); } return plansys2_msgs::msg::Tree(); @@ -1296,7 +1311,7 @@ STNBTBuilder::get_node_dotgraph( { std::stringstream ss; ss << t(2) << node->node_num << " [label=\""; - ss << parser::pddl::nameActionsToString(node->action.action); + ss << node->action.action.get_action_string(); ss << " " << to_string(node->action.type) << "\""; ss << "labeljust=c,style=filled"; @@ -1329,7 +1344,7 @@ STNBTBuilder::get_action_status( ActionStamped action_stamped, std::shared_ptr> action_map) { - auto index = "(" + parser::pddl::nameActionsToString(action_stamped.action) + "):" + + auto index = "(" + action_stamped.action.get_action_string() + "):" + std::to_string(static_cast(action_stamped.time * 1000)); if (action_map->find(index) != action_map->end()) { if ((*action_map)[index].action_executor) { @@ -1412,8 +1427,8 @@ STNBTBuilder::print_node(const plansys2::Node::Ptr node, int level) const } else { std::cerr << node->action.time + node->action.duration; } - std::cerr << ": (" << node->action.action->name; - for (const auto & param : node->action.action->parameters) { + std::cerr << ": (" << node->action.action.get_action_name(); + for (const auto & param : node->action.action.get_action_params()) { std::cerr << " " << param.name; } std::cerr << ")_" << to_string(node->action.type); diff --git a/plansys2_executor/test/pddl/factory4.pddl b/plansys2_executor/test/pddl/factory4.pddl new file mode 100644 index 00000000..b9c05b8c --- /dev/null +++ b/plansys2_executor/test/pddl/factory4.pddl @@ -0,0 +1,73 @@ +( define (domain factory) +( :requirements :strips :adl :typing :fluents ) +( :types + robot - object + zone - object + piece - object + car - object +) +( :predicates + ( robot_available ?robot0 - robot ) + ( battery_full ?robot0 - robot ) + ( robot_at ?robot0 - robot ?zone1 - zone ) + ( piece_at ?piece0 - piece ?zone1 - zone ) + ( piece_is_wheel ?piece0 - piece ) + ( piece_is_body_car ?piece0 - piece ) + ( piece_is_steering_wheel ?piece0 - piece ) + ( piece_not_used ?piece0 - piece ) + ( is_assembly_zone ?zone0 - zone ) + ( car_assembled ?car0 - car ) +) +( :action move + :parameters (?robot0 - robot ?zone1 - zone ?zone2 - zone) + :precondition (and + (robot_available ?robot0) + (robot_at ?robot0 ?zone1) + (battery_full ?robot0) + ) + :effect (and + (not (robot_at ?robot0 ?zone1)) + (robot_at ?robot0 ?zone2) + (robot_available ?robot0) + ) +) +( :action transport + :parameters (?robot0 - robot ?piece1 - piece ?zone2 - zone ?zone3 - zone) + :precondition (and + (robot_available ?robot0) + (robot_at ?robot0 ?zone2) + (piece_at ?piece1 ?zone2) + ) + :effect (and + (not (robot_at ?robot0 ?zone2)) + (not (piece_at ?piece1 ?zone2)) + (robot_at ?robot0 ?zone3) + (piece_at ?piece1 ?zone3) + (robot_available ?robot0) + ) +) +( :action assemble + :parameters (?robot0 - robot ?zone1 - zone ?piece2 - piece ?piece3 - piece ?piece4 - piece ?car5 - car) + :precondition (and + (robot_available ?robot0) + (is_assembly_zone ?zone1) + (robot_at ?robot0 ?zone1) + (piece_at ?piece2 ?zone1) + (piece_at ?piece3 ?zone1) + (piece_at ?piece4 ?zone1) + (piece_not_used ?piece2) + (piece_not_used ?piece3) + (piece_not_used ?piece4) + (piece_is_wheel ?piece2) + (piece_is_body_car ?piece3) + (piece_is_steering_wheel ?piece4) + ) + :effect (and + (not (piece_not_used ?piece2)) + (not (piece_not_used ?piece3)) + (not (piece_not_used ?piece4)) + (car_assembled ?car5) + (robot_available ?robot0) + ) +) +) diff --git a/plansys2_executor/test/unit/bt_node_test.cpp b/plansys2_executor/test/unit/bt_node_test.cpp index bb83211b..bff95d69 100644 --- a/plansys2_executor/test/unit/bt_node_test.cpp +++ b/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>(); (*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.action.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>(); (*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.action.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>(); (*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.action.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>(); (*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.action.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>(); (*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.action.index(), + std::variant_npos); std::string bt_xml_tree = R"( diff --git a/plansys2_executor/test/unit/bt_node_test_charging.cpp b/plansys2_executor/test/unit/bt_node_test_charging.cpp index 255643e1..38b63b54 100644 --- a/plansys2_executor/test/unit/bt_node_test_charging.cpp +++ b/plansys2_executor/test/unit/bt_node_test_charging.cpp @@ -102,14 +102,14 @@ TEST(problem_expert, wait_atstart_req_test) auto action_map = std::make_shared>(); (*action_map)["(move robot1 wp1 wp2):5"] = plansys2::ActionExecutionInfo(); - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = + (*action_map)["(move robot1 wp1 wp2):5"].action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move robot1 wp1 wp2)"), plansys2::get_action_params("(move robot1 wp1 wp2)")); ASSERT_NE( - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wp1 wp2):5"].action_info.action.index(), + std::variant_npos); std::string bt_xml_tree = R"( @@ -227,14 +227,14 @@ TEST(problem_expert, apply_atstart_effect_test) auto action_map = std::make_shared>(); (*action_map)["(move robot1 wp1 wp2):5"] = plansys2::ActionExecutionInfo(); - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = + (*action_map)["(move robot1 wp1 wp2):5"].action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move robot1 wp1 wp2)"), plansys2::get_action_params("(move robot1 wp1 wp2)")); ASSERT_NE( - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wp1 wp2):5"].action_info.action.index(), + std::variant_npos); std::string bt_xml_tree = R"( @@ -354,14 +354,14 @@ TEST(problem_expert, apply_atend_effect_test) auto action_map = std::make_shared>(); (*action_map)["(move robot1 wp1 wp2):5"] = plansys2::ActionExecutionInfo(); - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = + (*action_map)["(move robot1 wp1 wp2):5"].action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move robot1 wp1 wp2)"), plansys2::get_action_params("(move robot1 wp1 wp2)")); ASSERT_NE( - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wp1 wp2):5"].action_info.action.index(), + std::variant_npos); std::string bt_xml_tree = R"( diff --git a/plansys2_executor/test/unit/executor_test.cpp b/plansys2_executor/test/unit/executor_test.cpp index f3854bc0..e4491a12 100644 --- a/plansys2_executor/test/unit/executor_test.cpp +++ b/plansys2_executor/test/unit/executor_test.cpp @@ -715,7 +715,7 @@ TEST(executor, action_real_action_1) (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_executor = plansys2::ActionExecutor::make_shared( "(move r2d2 steering_wheels_zone assembly_zone)", test_lf_node); - (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].durative_action_info = + (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move r2d2 steering_wheels_zone assembly_zone):0"), plansys2::get_action_params("(move r2d2 steering_wheels_zone assembly_zone):0")); @@ -756,8 +756,8 @@ TEST(executor, action_real_action_1) R"( - + @@ -876,6 +876,261 @@ TEST(executor, action_real_action_1) t.join(); } +TEST(executor, action_real_action_2) +{ + auto test_node = rclcpp::Node::make_shared("action_real_action_2"); + auto test_lf_node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lf_node"); + auto domain_node = std::make_shared(); + auto problem_node = std::make_shared(); + auto planner_node = std::make_shared(); + + auto move_action_node = MoveAction::make_shared("move_action_performer", 100ms); + move_action_node->set_parameter({"action_name", "move"}); + + auto domain_client = std::make_shared(); + auto problem_client = std::make_shared(); + auto planner_client = std::make_shared(); + + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_executor"); + + domain_node->set_parameter({"model_file", pkgpath + "/pddl/factory4.pddl"}); + problem_node->set_parameter({"model_file", pkgpath + "/pddl/factory4.pddl"}); + + rclcpp::executors::SingleThreadedExecutor exe; + + exe.add_node(domain_node->get_node_base_interface()); + exe.add_node(problem_node->get_node_base_interface()); + exe.add_node(planner_node->get_node_base_interface()); + exe.add_node(move_action_node->get_node_base_interface()); + exe.add_node(test_lf_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); + move_action_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + test_lf_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + planner_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); + planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + test_lf_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("r2d2", "robot"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("steering_wheels_zone", "zone"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("body_car_zone", "zone"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); + + std::string bt_xml_tree = + R"( + + + + + + + + + + + + + + + + )"; + + auto action_map = std::make_shared>(); + (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"] = + plansys2::ActionExecutionInfo(); + (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_executor = + plansys2::ActionExecutor::make_shared( + "(move r2d2 steering_wheels_zone assembly_zone)", test_lf_node); + (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_info = + domain_client->getAction( + plansys2::get_action_name("(move r2d2 steering_wheels_zone assembly_zone):0"), + plansys2::get_action_params("(move r2d2 steering_wheels_zone assembly_zone):0")); + + auto blackboard = BT::Blackboard::create(); + + blackboard->set("action_map", action_map); + blackboard->set("node", test_lf_node); + blackboard->set("domain_client", domain_client); + blackboard->set("problem_client", problem_client); + + BT::BehaviorTreeFactory factory; + factory.registerNodeType("ExecuteAction"); + factory.registerNodeType("WaitAction"); + factory.registerNodeType("CheckOverAllReq"); + factory.registerNodeType("WaitAtStartReq"); + factory.registerNodeType("CheckAtEndReq"); + factory.registerNodeType("ApplyAtStartEffect"); + factory.registerNodeType("ApplyAtEndEffect"); + + // Test WaitAtStartReq + try { + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + + for (int i = 0; i < 10; i++) { + status = tree.tickOnce(); + ASSERT_EQ(status, BT::NodeStatus::RUNNING); + ASSERT_EQ(WaitActionTest::test_status, BT::NodeStatus::RUNNING); + } + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } + + // Test ApplyAtStartEffect and CheckOverAllReq + bt_xml_tree = + R"( + + + + + + + + + + + + + + + )"; + + + std::vector predicates = { + "(robot_at r2d2 steering_wheels_zone)", + "(robot_available r2d2)", + "(battery_full r2d2)", + }; + + for (const auto & pred : predicates) { + ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); + } + + try { + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + + ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); + status = tree.tickOnce(); + + ASSERT_EQ(ApplyAtStartEffectTest::test_status, BT::NodeStatus::SUCCESS); + ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); + ASSERT_TRUE( + problem_client->existPredicate( + plansys2::Predicate( + "(robot_at r2d2 steering_wheels_zone)"))); + + status = tree.tickOnce(); + ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); + ASSERT_EQ(ExecuteActionTest::test_status, BT::NodeStatus::RUNNING); + status = tree.tickOnce(); + ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); + ASSERT_EQ(ExecuteActionTest::test_status, BT::NodeStatus::RUNNING); + + ASSERT_TRUE(problem_client->removePredicate(plansys2::Predicate("(battery_full r2d2)"))); + + status = tree.tickOnce(); + ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::FAILURE); + ASSERT_EQ(status, BT::NodeStatus::FAILURE); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } + + // Test CheckAtEndReq and ApplyAtEndEffect + (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].at_start_effects_applied = + false; + + ApplyAtStartEffectTest::test_status = BT::NodeStatus::IDLE; + + for (const auto & pred : predicates) { + ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); + } + + try { + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + + ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); + + while (ApplyAtStartEffectTest::test_status != BT::NodeStatus::SUCCESS) { + status = tree.tickOnce(); + } + + ASSERT_FALSE( + problem_client->existPredicate( + plansys2::Predicate( + "(robot_at r2d2 assembly_zone)"))); + ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); + + while (ExecuteActionTest::test_status != BT::NodeStatus::SUCCESS) { + status = tree.tickOnce(); + ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); + } + + while (ApplyAtEndEffectTest::test_status != BT::NodeStatus::SUCCESS) { + status = tree.tickOnce(); + } + + ASSERT_TRUE( + problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 assembly_zone)"))); + ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } + + + ExecuteActionTest::test_status = BT::NodeStatus::IDLE; + WaitActionTest::test_status = BT::NodeStatus::IDLE; + CheckOverAllReqTest::test_status = BT::NodeStatus::IDLE; + WaitAtStartReqTest::test_status = BT::NodeStatus::IDLE; + CheckAtEndReqTest::test_status = BT::NodeStatus::IDLE; + ApplyAtStartEffectTest::test_status = BT::NodeStatus::IDLE; + ApplyAtEndEffectTest::test_status = BT::NodeStatus::IDLE; + + ExecuteActionTest::halted_ = false; + WaitActionTest::halted_ = false; + CheckOverAllReqTest::halted_ = false; + WaitAtStartReqTest::halted_ = false; + CheckAtEndReqTest::halted_ = false; + ApplyAtStartEffectTest::halted_ = false; + ApplyAtEndEffectTest::halted_ = false; + + finish = true; + t.join(); +} + TEST(executor, cancel_bt_execution) { auto test_node = rclcpp::Node::make_shared("action_real_action_1"); @@ -969,7 +1224,7 @@ TEST(executor, cancel_bt_execution) (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_executor = plansys2::ActionExecutor::make_shared( "(move r2d2 steering_wheels_zone assembly_zone)", test_lf_node); - (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].durative_action_info = + (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move r2d2 steering_wheels_zone assembly_zone):0"), plansys2::get_action_params("(move r2d2 steering_wheels_zone assembly_zone):0")); @@ -1224,6 +1479,152 @@ TEST(executor, executor_client_execute_plan) t.join(); } +TEST(executor, executor_client_execute_plan_2) +{ + auto test_node_1 = rclcpp::Node::make_shared("test_node_1"); + auto test_node_2 = rclcpp::Node::make_shared("test_node_2"); + auto test_node_3 = rclcpp::Node::make_shared("test_node_3"); + auto test_lf_node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lf_node"); + auto domain_node = std::make_shared(); + auto problem_node = std::make_shared(); + auto planner_node = std::make_shared(); + auto executor_node = std::make_shared(); + + auto move_action_node = MoveAction::make_shared("move_action_performer", 100ms); + move_action_node->set_parameter({"action_name", "move"}); + + auto domain_client = std::make_shared(); + auto problem_client = std::make_shared(); + auto planner_client = std::make_shared(); + auto executor_client = std::make_shared(); + + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_executor"); + + domain_node->set_parameter({"model_file", pkgpath + "/pddl/factory4.pddl"}); + problem_node->set_parameter({"model_file", pkgpath + "/pddl/factory4.pddl"}); + + rclcpp::executors::SingleThreadedExecutor exe; + + exe.add_node(domain_node->get_node_base_interface()); + exe.add_node(problem_node->get_node_base_interface()); + exe.add_node(planner_node->get_node_base_interface()); + exe.add_node(executor_node->get_node_base_interface()); + exe.add_node(move_action_node->get_node_base_interface()); + exe.add_node(test_lf_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); + planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + move_action_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + test_lf_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + executor_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + { + rclcpp::Rate rate(10); + auto start = test_node_1->now(); + while ((test_node_1->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); + planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + executor_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + test_lf_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + { + rclcpp::Rate rate(10); + auto start = test_node_1->now(); + while ((test_node_1->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("r2d2", "robot"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("steering_wheels_zone", "zone"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("body_car_zone", "zone"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); + + std::vector predicates = { + "(robot_at r2d2 steering_wheels_zone)", + "(robot_available r2d2)", + "(battery_full r2d2)", + }; + + for (const auto & pred : predicates) { + ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); + } + problem_client->setGoal(plansys2::Goal("(and(robot_at r2d2 assembly_zone))")); + + auto domain = domain_client->getDomain(); + auto problem = problem_client->getProblem(); + auto plan = planner_client->getPlan(domain, problem); + ASSERT_FALSE(domain.empty()); + ASSERT_FALSE(problem.empty()); + ASSERT_TRUE(plan.has_value()); + + { + rclcpp::Rate rate(10); + auto start = test_node_1->now(); + + ASSERT_TRUE(executor_client->start_plan_execution(plan.value())); + while (rclcpp::ok() && executor_client->execute_and_check_plan()) { + auto feedback = executor_client->getFeedBack(); + ASSERT_LT(feedback.action_execution_status.size(), 3); + rate.sleep(); + } + } + + ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 assembly_zone)"))); + + ASSERT_TRUE(executor_client->getResult().has_value()); + auto result = executor_client->getResult().value(); + + ASSERT_TRUE(result.success); + ASSERT_EQ(result.action_execution_status.size(), 2u); + for (const auto & action_status : result.action_execution_status) { + ASSERT_EQ(action_status.status, plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED); + } + problem_client->setGoal(plansys2::Goal("(and(robot_at r2d2 body_car_zone))")); + + problem = problem_client->getProblem(); + plan = planner_client->getPlan(domain, problem); + ASSERT_FALSE(problem.empty()); + ASSERT_TRUE(plan.has_value()); + { + rclcpp::Rate rate(10); + auto start = test_node_1->now(); + + ASSERT_TRUE(executor_client->start_plan_execution(plan.value())); + while (rclcpp::ok() && executor_client->execute_and_check_plan()) { + auto feedback = executor_client->getFeedBack(); + ASSERT_LT(feedback.action_execution_status.size(), 3); + rate.sleep(); + } + } + + ASSERT_TRUE( + problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 body_car_zone)"))); + + ASSERT_TRUE(executor_client->getResult().has_value()); + result = executor_client->getResult().value(); + for (const auto & action_status : result.action_execution_status) { + ASSERT_EQ(action_status.status, plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED); + } + + finish = true; + t.join(); +} + class PatrolAction : public plansys2::ActionExecutorClient { public: diff --git a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp index b6e61cac..a42abaaf 100644 --- a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp +++ b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp @@ -231,15 +231,16 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_EQ(action_sequence.size(), 6u); ASSERT_NEAR(action_sequence[0].time, 0.000, 0.0001); - ASSERT_EQ(action_sequence[0].action->name, "askcharge"); - ASSERT_EQ(action_sequence[0].action->parameters[0].name, "leia"); - ASSERT_EQ(action_sequence[0].action->parameters[1].name, "entrance"); - ASSERT_EQ(action_sequence[0].action->parameters[2].name, "chargingroom"); + ASSERT_EQ(action_sequence[0].action.get_action_name(), "askcharge"); + ASSERT_EQ(action_sequence[0].action.get_action_params()[0].name, "leia"); + ASSERT_EQ(action_sequence[0].action.get_action_params()[1].name, "entrance"); + ASSERT_EQ(action_sequence[0].action.get_action_params()[2].name, "chargingroom"); ASSERT_EQ( - parser::pddl::toString(action_sequence[0].action->at_start_effects), + parser::pddl::toString(action_sequence[0].action.get_at_start_effects()), "(and (not (robot_at leia entrance))(robot_at leia chargingroom))"); std::vector action_0_predicates; - parser::pddl::getPredicates(action_0_predicates, action_sequence[0].action->at_start_effects); + parser::pddl::getPredicates( + action_0_predicates, action_sequence[0].action.get_at_start_effects()); ASSERT_EQ(action_0_predicates.size(), 2u); ASSERT_EQ(action_0_predicates[0].name, "robot_at"); ASSERT_EQ(action_0_predicates[0].parameters[0].name, "leia"); @@ -248,28 +249,28 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_TRUE( plansys2::check( - action_sequence[0].action->at_start_requirements, + action_sequence[0].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( plansys2::check( - action_sequence[1].action->at_start_requirements, + action_sequence[1].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( plansys2::check( - action_sequence[2].action->at_start_requirements, + action_sequence[2].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( plansys2::check( - action_sequence[3].action->at_start_requirements, + action_sequence[3].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( plansys2::check( - action_sequence[4].action->at_start_requirements, + action_sequence[4].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( plansys2::check( - action_sequence[5].action->at_start_requirements, + action_sequence[5].action.get_at_start_requirements(), problem_client)); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[0], predicates, functions)); @@ -293,10 +294,10 @@ TEST(simple_btbuilder_tests, test_plan_1) parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"))), predicates.end()); plansys2::apply( - action_sequence[0].action->at_start_effects, + action_sequence[0].action.get_at_start_effects(), predicates, functions); plansys2::apply( - action_sequence[0].action->at_end_effects, + action_sequence[0].action.get_at_end_effects(), predicates, functions); ASSERT_EQ( @@ -318,10 +319,10 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); plansys2::apply( - action_sequence[1].action->at_start_effects, + action_sequence[1].action.get_at_start_effects(), predicates, functions); plansys2::apply( - action_sequence[1].action->at_end_effects, + action_sequence[1].action.get_at_end_effects(), predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[2], predicates, functions)); @@ -329,37 +330,37 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); plansys2::apply( - action_sequence[2].action->at_start_effects, + action_sequence[2].action.get_at_start_effects(), predicates, functions); plansys2::apply( - action_sequence[2].action->at_end_effects, + action_sequence[2].action.get_at_end_effects(), predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[3], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); plansys2::apply( - action_sequence[3].action->at_start_effects, + action_sequence[3].action.get_at_start_effects(), predicates, functions); plansys2::apply( - action_sequence[3].action->at_end_effects, + action_sequence[3].action.get_at_end_effects(), predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); plansys2::apply( - action_sequence[4].action->at_start_effects, + action_sequence[4].action.get_at_start_effects(), predicates, functions); plansys2::apply( - action_sequence[4].action->at_end_effects, + action_sequence[4].action.get_at_end_effects(), predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); plansys2::apply( - action_sequence[5].action->at_start_effects, + action_sequence[5].action.get_at_start_effects(), predicates, functions); plansys2::apply( - action_sequence[5].action->at_end_effects, + action_sequence[5].action.get_at_end_effects(), predicates, functions); ASSERT_NE( @@ -526,10 +527,10 @@ TEST(simple_btbuilder_tests, test_plan_2) // Apply roots actions for (auto & action_node : roots) { plansys2::apply( - action_node->action.action->at_start_effects, + action_node->action.action.get_at_start_effects(), predicates, functions); plansys2::apply( - action_node->action.action->at_end_effects, + action_node->action.action.get_at_end_effects(), predicates, functions); } @@ -559,11 +560,11 @@ TEST(simple_btbuilder_tests, test_plan_2) plansys2_msgs::msg::Node::AND); auto node_satisfy_1 = btbuilder->get_node_satisfy(tree, *roots.begin(), nullptr); ASSERT_NE(node_satisfy_1, nullptr); - ASSERT_EQ(node_satisfy_1->action.action->name, "move"); - ASSERT_EQ(node_satisfy_1->action.action->parameters.size(), 3u); - ASSERT_EQ(node_satisfy_1->action.action->parameters[0].name, "robot1"); - ASSERT_EQ(node_satisfy_1->action.action->parameters[1].name, "assembly_zone"); - ASSERT_EQ(node_satisfy_1->action.action->parameters[2].name, "body_car_zone"); + ASSERT_EQ(node_satisfy_1->action.action.get_action_name(), "move"); + ASSERT_EQ(node_satisfy_1->action.action.get_action_params().size(), 3u); + ASSERT_EQ(node_satisfy_1->action.action.get_action_params()[0].name, "robot1"); + ASSERT_EQ(node_satisfy_1->action.action.get_action_params()[1].name, "assembly_zone"); + ASSERT_EQ(node_satisfy_1->action.action.get_action_params()[2].name, "body_car_zone"); auto it = roots.begin(); it++;