From ab1cdcc74387a05bd7e9ac9df8d9dff42baebfc0 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Mon, 16 Sep 2024 19:23:52 +0200 Subject: [PATCH] execute regular actions --- .../plansys2_executor/ActionExecutor.hpp | 5 ++- .../src/plansys2_executor/ComputeBT.cpp | 17 ++++++-- .../src/plansys2_executor/ExecutorNode.cpp | 41 ++++++++++++++----- .../behavior_tree/apply_atend_effect_node.cpp | 14 ++++++- .../apply_atstart_effect_node.cpp | 9 +++- .../behavior_tree/check_atend_req_node.cpp | 11 ++++- .../behavior_tree/check_overall_req_node.cpp | 15 ++++++- .../behavior_tree/wait_atstart_req_node.cpp | 23 +++++++++-- plansys2_executor/test/unit/bt_node_test.cpp | 30 +++++++------- .../test/unit/bt_node_test_charging.cpp | 18 ++++---- plansys2_executor/test/unit/executor_test.cpp | 4 +- 11 files changed, 135 insertions(+), 52 deletions(-) diff --git a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp index 912b472d..59dd70f5 100644 --- a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp +++ b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp @@ -19,6 +19,7 @@ #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" @@ -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 durative_action_info = {nullptr}; + std::variant< + std::shared_ptr, + std::shared_ptr> action_info; std::string execution_error_info; double duration; double duration_overrun_percentage = -1.0; diff --git a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp index 27e05150..22f06fa9 100644 --- a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp +++ b/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() && diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index f444863d..e9306805 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/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 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 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 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 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) { 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..e841e44e 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,10 +17,15 @@ #include #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; +using shared_ptr_durative = std::shared_ptr; + + 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((*action_map_)[action].action_info)) { + effect = std::get( + (*action_map_)[action].action_info)->effects; + } else if (std::holds_alternative((*action_map_)[action].action_info)) { + effect = std::get( + (*action_map_)[action].action_info)->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..944cd2f6 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 @@ -21,6 +21,8 @@ namespace plansys2 { +using shared_ptr_action = std::shared_ptr; + 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((*action_map_)[action].action_info)) { + return BT::NodeStatus::SUCCESS; + } + + auto effect = std::get>( + (*action_map_)[action].action_info)->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..3fe6f6cb 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 @@ -22,6 +22,8 @@ namespace plansys2 { +using shared_ptr_action = std::shared_ptr; + CheckAtEndReq::CheckAtEndReq( const std::string & xml_tag_name, const BT::NodeConfig & conf) @@ -44,7 +46,12 @@ CheckAtEndReq::tick() auto node = config().blackboard->get("node"); - auto reqs = (*action_map_)[action].durative_action_info->at_end_requirements; + if (std::holds_alternative((*action_map_)[action].action_info)) { + return BT::NodeStatus::SUCCESS; + } + + auto reqs = std::get>( + (*action_map_)[action].action_info)->at_end_requirements; if (!check(reqs, problem_client_)) { (*action_map_)[action].execution_error_info = "Error checking at end requirements"; @@ -52,7 +59,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..3e7e4e8d 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,10 +18,14 @@ #include #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; +using shared_ptr_durative = std::shared_ptr; + CheckOverAllReq::CheckOverAllReq( const std::string & xml_tag_name, const BT::NodeConfig & conf) @@ -44,7 +48,14 @@ CheckOverAllReq::tick() auto node = config().blackboard->get("node"); - auto reqs = (*action_map_)[action].durative_action_info->over_all_requirements; + plansys2_msgs::msg::Tree reqs; + if (std::holds_alternative((*action_map_)[action].action_info)) { + reqs = std::get( + (*action_map_)[action].action_info)->preconditions; + } else if (std::holds_alternative((*action_map_)[action].action_info)) { + reqs = std::get( + (*action_map_)[action].action_info)->over_all_requirements; + } if (!check(reqs, problem_client_)) { (*action_map_)[action].execution_error_info = "Error checking over all requirements"; @@ -52,7 +63,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..fc1fa189 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,10 +18,14 @@ #include #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; +using shared_ptr_durative = std::shared_ptr; + WaitAtStartReq::WaitAtStartReq( const std::string & xml_tag_name, const BT::NodeConfig & conf) @@ -44,8 +48,19 @@ 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; + plansys2_msgs::msg::Tree reqs_as; + plansys2_msgs::msg::Tree reqs_oa; + if (std::holds_alternative((*action_map_)[action].action_info)) { + reqs_as = std::get( + (*action_map_)[action].action_info)->preconditions; + reqs_oa = std::get( + (*action_map_)[action].action_info)->preconditions; + } else if (std::holds_alternative((*action_map_)[action].action_info)) { + reqs_as = std::get( + (*action_map_)[action].action_info)->at_start_requirements; + reqs_oa = std::get( + (*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; } diff --git a/plansys2_executor/test/unit/bt_node_test.cpp b/plansys2_executor/test/unit/bt_node_test.cpp index bb83211b..245bac1d 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.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.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.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.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.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..592ee0a9 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.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.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.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..3121a8d5 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")); @@ -969,7 +969,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"));