diff --git a/plansys2_core/include/plansys2_core/State.hpp b/plansys2_core/include/plansys2_core/State.hpp index 82f49f09a..73d6525fa 100644 --- a/plansys2_core/include/plansys2_core/State.hpp +++ b/plansys2_core/include/plansys2_core/State.hpp @@ -248,48 +248,48 @@ class State return functions_.erase(std::move(function)) == 1; } - bool hasInstance(const plansys2::Instance & instance) + bool hasInstance(const plansys2::Instance & instance) const { return instances_.find(instance) != instances_.end(); } - bool hasInstance(plansys2::Instance && instance) + bool hasInstance(plansys2::Instance && instance) const { return instances_.find(std::move(instance)) != instances_.end(); } - bool hasPredicate(const std::string & predicate_str) + bool hasPredicate(const std::string & predicate_str) const { auto predicate = parser::pddl::fromStringPredicate(predicate_str); return predicates_.find(predicate) != predicates_.end(); } - bool hasPredicate(const plansys2::Predicate & predicate) + bool hasPredicate(const plansys2::Predicate & predicate) const { return predicates_.find(predicate) != predicates_.end(); } - bool hasPredicate(plansys2::Predicate && predicate) + bool hasPredicate(plansys2::Predicate && predicate) const { return predicates_.find(std::move(predicate)) != predicates_.end(); } - bool hasInferredPredicate(const std::string & predicate_str) + bool hasInferredPredicate(const std::string & predicate_str) const { auto predicate = parser::pddl::fromStringPredicate(predicate_str); return inferred_predicates_.find(predicate) != inferred_predicates_.end(); } - bool hasInferredPredicate(const plansys2::Predicate & predicate) + bool hasInferredPredicate(const plansys2::Predicate & predicate) const { return inferred_predicates_.find(predicate) != inferred_predicates_.end(); } - bool hasInferredPredicate(plansys2::Predicate && predicate) + bool hasInferredPredicate(plansys2::Predicate && predicate) const { return inferred_predicates_.find(std::move(predicate)) != inferred_predicates_.end(); } - bool hasFunction(const plansys2::Function & function) + bool hasFunction(const plansys2::Function & function) const { return functions_.find(function) != functions_.end(); } - bool hasFunction(plansys2::Function && function) + bool hasFunction(plansys2::Function && function) const { return functions_.find(std::move(function)) != functions_.end(); } @@ -315,6 +315,18 @@ class State return functions_.find(std::move(function)); } + bool updateFunctionValue(const plansys2::Function & function, const double & value) + { + auto func_it = functions_.find(function); + if (func_it != functions_.end()) { + plansys2::Function updated_func = *func_it; + removeFunction(func_it); + updated_func.value = value; + return addFunction(updated_func); + } + return false; + } + void setDerivedPredicates(const plansys2::DerivedResolutionGraph derived_predicates) { derived_predicates_ = derived_predicates; diff --git a/plansys2_executor/CMakeLists.txt b/plansys2_executor/CMakeLists.txt index cb485a7da..8a1886d5a 100644 --- a/plansys2_executor/CMakeLists.txt +++ b/plansys2_executor/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(pluginlib REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(OpenMP REQUIRED) set(EXECUTOR_SOURCES src/plansys2_executor/ExecutorClient.cpp @@ -61,6 +62,7 @@ target_link_libraries(${PROJECT_NAME} rclcpp_action::rclcpp_action rclcpp_cascade_lifecycle::rclcpp_cascade_lifecycle rclcpp_lifecycle::rclcpp_lifecycle + OpenMP::OpenMP_CXX ${std_msgs_TARGETS} ${std_srvs_TARGETS} PRIVATE diff --git a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp index 2a79a8e19..0218de1ef 100644 --- a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp +++ b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,29 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +namespace std +{ +template +struct hash> +{ + std::size_t operator()(const std::pair & p) const + { + std::size_t seed = 0; + hash_combine(seed, p.first); + hash_combine(seed, p.second); + return seed; + } + +private: + // Hash combine function + template + inline void hash_combine(std::size_t & seed, const T & value) const + { + seed ^= std::hash{}(value) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } +}; +} // namespace std + namespace plansys2 { @@ -53,8 +77,7 @@ struct ActionNode int node_num; int level_num; - std::vector predicates; - std::vector functions; + plansys2::State state; std::list in_arcs; std::list out_arcs; @@ -181,6 +204,11 @@ class SimpleBTBuilder : public BTBuilder */ void prune_forward(ActionNode::Ptr current, std::list & used_nodes); + void get_state_recursive( + const ActionNode::Ptr & node, + std::list & used_nodes, + plansys2::State & state); + /** * @brief Computes the state of the world at a node. * @@ -189,41 +217,44 @@ class SimpleBTBuilder : public BTBuilder * * @param[in] node The node to compute state for. * @param[in,out] used_nodes List of nodes already processed. - * @param[out] predicates Resulting predicates at this node. - * @param[out] functions Resulting functions at this node. + * @param[out] state Resulting state at this node. */ - void get_state( + plansys2::State get_state( const ActionNode::Ptr & node, std::list & used_nodes, - std::vector & predicates, - std::vector & functions) const; + const plansys2::State & state); /** * @brief Checks if an action is executable in a given state. * * @param[in] action The action to check. - * @param[in] predicates Current predicates. - * @param[in] functions Current functions. + * @param[in] state Current state. * @return True if the action's requirements are satisfied, false otherwise. */ bool is_action_executable( const ActionStamped & action, - std::vector & predicates, - std::vector & functions) const; + const plansys2::State & state) const; + + std::vector check_requirements( + const std::vector & requirements, + std::shared_ptr & graph, + std::shared_ptr & new_node); + bool check_requirement( + const plansys2_msgs::msg::Tree & requirement, + std::shared_ptr & graph, + std::shared_ptr & new_node); /** * @brief Finds root actions that can be executed immediately. * * @param[in,out] action_sequence Actions to check, executable ones will be removed. - * @param[in] predicates Current predicates. - * @param[in] functions Current functions. + * @param[in] state Current state. * @param[in,out] node_counter Counter for assigning unique node IDs. * @return std::list List of nodes representing executable actions. */ std::list get_roots( std::vector & action_sequence, - std::vector & predicates, - std::vector & functions, + const plansys2::State & state, int & node_counter); /** @@ -279,28 +310,28 @@ class SimpleBTBuilder : public BTBuilder * @brief Removes requirements that are already satisfied in the current state. * * @param[in,out] requirements List of requirements to filter. - * @param[in] predicates Current predicates. - * @param[in] functions Current functions. + * @param[in] state Current state. */ void remove_existing_requirements( std::vector & requirements, - std::vector & predicates, - std::vector & functions) const; + const plansys2::State & state) const; /** * @brief Checks if an action can be executed in parallel with a set of other actions. * * @param[in] action The action to check. - * @param[in] predicates Current predicates. - * @param[in] functions Current functions. + * @param[in] state Current state. * @param[in] ret List of actions to check parallelization with. * @return True if the action can be executed in parallel, false otherwise. */ bool is_parallelizable( const plansys2::ActionStamped & action, - const std::vector & predicates, - const std::vector & functions, - const std::list & ret) const; + const plansys2::State & state, + const std::list & ret); + + void apply_action_to_state( + const plansys2::ActionStamped & action, + plansys2::State & state); /** * @brief Generates the behavior tree XML for a node and its descendants. @@ -436,6 +467,10 @@ class SimpleBTBuilder : public BTBuilder ActionGraph::Ptr graph_; std::string bt_; std::string bt_action_; + + std::unordered_map< + std::pair, plansys2::State> apply_action_state_cache_; + std::unordered_map, bool> check_action_state_cache_; }; } // namespace plansys2 diff --git a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp index 3e98053f8..394671ed8 100644 --- a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp +++ b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp @@ -26,6 +26,7 @@ #include #include +#include "plansys2_core/State.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/BTBuilder.hpp" @@ -35,18 +36,6 @@ namespace plansys2 { -/** - * @brief Structure that represents a state vector in the planning system. - * - * Contains the predicates and functions that define a state of the world - * at a specific point in time during plan execution. - */ -struct StateVec -{ - std::vector predicates; - std::vector functions; -}; - /** * @class plansys2::STNBTBuilder * @brief Behavior tree builder that uses Simple Temporal Networks (STN) to manage temporal constraints. @@ -199,9 +188,9 @@ class STNBTBuilder : public BTBuilder * * @param[in] happenings The set of happening times. * @param[in] plan The simplified plan representation. - * @return std::map Map from happening times to state vectors. + * @return std::map Map from happening times to state vectors. */ - std::map get_states( + std::map get_states( const std::set & happenings, const std::multimap & plan) const; @@ -213,8 +202,7 @@ class STNBTBuilder : public BTBuilder * @return Tree representing the conjunction of predicates and functions. */ plansys2_msgs::msg::Tree from_state( - const std::vector & preds, - const std::vector & funcs) const; + const plansys2::State & state) const; /** * @brief Finds the graph nodes corresponding to an action. @@ -252,7 +240,7 @@ class STNBTBuilder : public BTBuilder const std::pair & action, const std::multimap & plan, const std::set & happenings, - const std::map & states) const; + const std::map & states) const; /** * @brief Finds actions that satisfy the requirements of an action. @@ -267,7 +255,7 @@ class STNBTBuilder : public BTBuilder const std::pair & action, const std::multimap & plan, const std::set & happenings, - const std::map & states) const; + const std::map & states) const; /** * @brief Finds actions that threaten the execution of an action. @@ -285,7 +273,7 @@ class STNBTBuilder : public BTBuilder const std::pair & action, const std::multimap & plan, const std::set & happenings, - const std::map & states) const; + const std::map & states) const; /** * @brief Checks if an action can be applied in a given state. @@ -303,7 +291,7 @@ class STNBTBuilder : public BTBuilder const std::pair & action, const std::multimap & plan, const int & time, - StateVec & state) const; + plansys2::State & state) const; /** * @brief Computes the difference between two states. @@ -312,9 +300,9 @@ class STNBTBuilder : public BTBuilder * * @param[in] X_1 The first state. * @param[in] X_2 The second state. - * @return StateVec containing the differences. + * @return plansys2::State containing the differences. */ - StateVec get_diff(const StateVec & X_1, const StateVec & X_2) const; + plansys2::State get_diff(const plansys2::State & X_1, const plansys2::State & X_2) const; /** * @brief Computes the intersection of two states. @@ -323,9 +311,9 @@ class STNBTBuilder : public BTBuilder * * @param[in] X_1 The first state. * @param[in] X_2 The second state. - * @return StateVec containing the intersection. + * @return plansys2::State containing the intersection. */ - StateVec get_intersection(const StateVec & X_1, const StateVec & X_2) const; + plansys2::State get_intersection(const plansys2::State & X_1, const plansys2::State & X_2) const; /** * @brief Gets the conditions required by an action. diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index 6e02192f4..412a3aa3c 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -272,14 +272,13 @@ void ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info) { auto goal = problem_client_->getGoal(); - auto local_predicates = problem_client_->getPredicates(); - auto local_functions = problem_client_->getFunctions(); + auto local_state = problem_client_->getState(); std::vector unordered_subgoals = parser::pddl::getSubtreeIds(goal); // just in case some goals are already satisfied for (auto it = unordered_subgoals.begin(); it != unordered_subgoals.end(); ) { - if (check(goal, local_predicates, local_functions, *it)) { + if (check(goal, local_state, *it)) { plansys2_msgs::msg::Tree new_goal; parser::pddl::fromString(new_goal, "(and " + parser::pddl::toString(goal, (*it)) + ")"); runtime_info.ordered_sub_goals.push_back(new_goal); @@ -296,18 +295,18 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info) std::shared_ptr action = domain_client_->getAction( action_name, get_action_params(plan_item.action)); - apply(action->effects, local_predicates, local_functions); + plansys2::apply(action->effects, local_state); } 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); + plansys2::apply(action->at_start_effects, local_state); + plansys2::apply(action->at_end_effects, local_state); } for (auto it = unordered_subgoals.begin(); it != unordered_subgoals.end(); ) { - if (check(goal, local_predicates, local_functions, *it)) { + if (check(goal, local_state, *it)) { plansys2_msgs::msg::Tree new_goal; parser::pddl::fromString(new_goal, "(and " + parser::pddl::toString(goal, (*it)) + ")"); runtime_info.ordered_sub_goals.push_back(new_goal); 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 80e99b0f9..1a3976654 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 @@ -46,7 +46,7 @@ ApplyAtEndEffect::tick() if (!(*action_map_)[action].at_end_effects_applied) { (*action_map_)[action].at_end_effects_applied = true; - apply(effect, problem_client_, 0); + plansys2::apply(effect, problem_client_); } return BT::NodeStatus::SUCCESS; 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 a18a10f9c..876906233 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 @@ -49,7 +49,7 @@ ApplyAtStartEffect::tick() if (!(*action_map_)[action].at_start_effects_applied) { (*action_map_)[action].at_start_effects_applied = true; - apply(effect, problem_client_, 0); + plansys2::apply(effect, problem_client_); } return BT::NodeStatus::SUCCESS; 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 c96662c41..86eb73a69 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 @@ -49,8 +49,11 @@ CheckAtEndReq::tick() } auto reqs = (*action_map_)[action].action_info.get_at_end_requirements(); + auto state = problem_client_->getState(); + state.addActionsAndPruneDerived({(*action_map_)[action].action_info}); + solveDerivedPredicates(state); - if (!check(reqs, problem_client_)) { + if (!check(reqs, state)) { (*action_map_)[action].execution_error_info = "Error checking at end requirements"; RCLCPP_ERROR_STREAM( 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 a077e1e3a..f9445b41a 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 @@ -52,9 +52,12 @@ CheckOverAllReq::tick() auto node = config().blackboard->get("node"); auto reqs = (*action_map_)[action].action_info.get_overall_requirements(); + auto state = problem_client_->getState(); + state.addActionsAndPruneDerived({(*action_map_)[action].action_info}); + solveDerivedPredicates(state); last_check_problem_ts_ = node->now(); - if (!check(reqs, problem_client_)) { + if (!check(reqs, state)) { (*action_map_)[action].execution_error_info = "Error checking over all requirements"; RCLCPP_ERROR_STREAM( diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/restore_atstart_effect_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/restore_atstart_effect_node.cpp index e4b7f9645..67ae7d8af 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/restore_atstart_effect_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/restore_atstart_effect_node.cpp @@ -52,8 +52,8 @@ RestoreAtStartEffect::tick() std::vector predicates; std::vector functions; - std::tuple ret = evaluate( - effect, problem_client_, predicates, functions, true, false, 0, true); + auto ret = apply( + effect, problem_client_, 0, true); } return BT::NodeStatus::SUCCESS; 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 34fa8d400..b5072d64a 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 @@ -54,7 +54,11 @@ WaitAtStartReq::tick() 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_); + auto state = problem_client_->getState(); + state.addActionsAndPruneDerived({(*action_map_)[action].action_info}); + solveDerivedPredicates(state); + + bool check_as = check(reqs_as, state); if (!check_as) { (*action_map_)[action].execution_error_info = "Error checking at start reqs"; @@ -66,7 +70,7 @@ WaitAtStartReq::tick() return BT::NodeStatus::RUNNING; } - bool check_oa = check(reqs_oa, problem_client_); + bool check_oa = check(reqs_oa, state); if (!check_oa) { (*action_map_)[action].execution_error_info = "Error checking over all reqs"; 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 5557079a0..9e74338ec 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 @@ -12,23 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include +#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" + #include +#include #include -#include #include -#include -#include +#include +#include #include +#include +#include +#include +#include -#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" - -#include "plansys2_problem_expert/Utils.hpp" #include "plansys2_pddl_parser/Utils.hpp" - +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" namespace plansys2 @@ -40,11 +39,8 @@ SimpleBTBuilder::SimpleBTBuilder() problem_client_ = std::make_shared(); } -void -SimpleBTBuilder::initialize( - const std::string & bt_action_1, - const std::string & bt_action_2, - int precision) +void SimpleBTBuilder::initialize( + const std::string & bt_action_1, const std::string & bt_action_2, int precision) { if (bt_action_1 != "") { bt_action_ = bt_action_1; @@ -64,25 +60,38 @@ WAIT_PREV_ACTIONS } } -bool -SimpleBTBuilder::is_action_executable( - const ActionStamped & action, - std::vector & predicates, - std::vector & functions) const +bool SimpleBTBuilder::is_action_executable( + const ActionStamped & action, const plansys2::State & state) const { if (action.action.is_action()) { - return check(action.action.get_overall_requirements(), predicates, functions); + return plansys2::check(action.action.get_overall_requirements(), state); + } + return plansys2::check(action.action.get_at_start_requirements(), state) && + plansys2::check(action.action.get_at_end_requirements(), state) && + plansys2::check(action.action.get_overall_requirements(), state); +} + +void SimpleBTBuilder::apply_action_to_state( + const plansys2::ActionStamped & action, plansys2::State & state) +{ + const std::string & action_str = action.action.get_action_string(); + auto action_key = std::make_pair(action_str, state); + + auto it = apply_action_state_cache_.find(action_key); + if (it != apply_action_state_cache_.end()) { + state = it->second; + return; } - 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); + if (action.action.is_durative_action()) { + plansys2::apply(action.action.get_at_start_effects(), state); + } + plansys2::apply(action.action.get_at_end_effects(), state); + apply_action_state_cache_.emplace(std::move(action_key), state); } -ActionNode::Ptr -SimpleBTBuilder::get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const ActionNode::Ptr & node, +ActionNode::Ptr SimpleBTBuilder::get_node_satisfy( + const plansys2_msgs::msg::Tree & requirement, const ActionNode::Ptr & node, const ActionNode::Ptr & current) { if (node == current) { @@ -92,20 +101,16 @@ SimpleBTBuilder::get_node_satisfy( ActionNode::Ptr ret = nullptr; // Get the state prior to applying the effects - auto predicates = node->predicates; - auto functions = node->functions; + plansys2::State state = node->state; // Is the requirement satisfied before applying the effects? - bool satisfied_before = check(requirement, predicates, functions); + bool satisfied_before = check(requirement, state); // Apply the effects - 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); + apply_action_to_state(node->action, state); // Is the requirement satisfied after applying the effects? - bool satisfied_after = check(requirement, predicates, functions); + bool satisfied_after = check(requirement, state); if (satisfied_after && !satisfied_before) { ret = node; @@ -123,10 +128,8 @@ SimpleBTBuilder::get_node_satisfy( return ret; } -void -SimpleBTBuilder::get_node_contradict( - const ActionNode::Ptr & node, - const ActionNode::Ptr & current, +void SimpleBTBuilder::get_node_contradict( + const ActionNode::Ptr & node, const ActionNode::Ptr & current, std::list & contradictions) { if (node == current) { @@ -134,18 +137,15 @@ SimpleBTBuilder::get_node_contradict( } // Get the state prior to applying the effects - auto predicates = node->predicates; - auto functions = node->functions; + auto state = node->state; // Are all of the requirements satisfied? - if (is_action_executable(current->action, predicates, functions)) { + if (is_action_executable(current->action, state)) { // Apply the effects - if (current->action.action.is_durative_action()) { - apply(current->action.action.get_at_start_effects(), predicates, functions); - } + apply_action_to_state(current->action, state); // Look for a contradiction - if (!is_action_executable(node->action, predicates, functions)) { + if (!is_action_executable(node->action, state)) { contradictions.push_back(node); } } @@ -156,23 +156,17 @@ SimpleBTBuilder::get_node_contradict( } } -bool -SimpleBTBuilder::is_parallelizable( - const plansys2::ActionStamped & action, - const std::vector & predicates, - const std::vector & functions, - const std::list & nodes) const +bool SimpleBTBuilder::is_parallelizable( + const plansys2::ActionStamped & action, const plansys2::State & state, + const std::list & nodes) { // Apply the "at start" effects of the new action. - auto preds = predicates; - auto funcs = functions; - if (action.action.is_durative_action()) { - apply(action.action.get_at_start_effects(), preds, funcs); - } + auto temp_state = state; + apply_action_to_state(action, temp_state); // Check the requirements of the actions in the input set. for (const auto & other : nodes) { - if (!is_action_executable(other->action, preds, funcs)) { + if (!is_action_executable(other->action, temp_state)) { return false; } } @@ -180,15 +174,12 @@ SimpleBTBuilder::is_parallelizable( // Apply the effects of the actions in the input set one at a time. for (const auto & other : nodes) { // Apply the "at start" effects of the action. - preds = predicates; - funcs = functions; + temp_state = state; - if (other->action.action.is_durative_action()) { - apply(other->action.action.get_at_start_effects(), preds, funcs); - } + apply_action_to_state(other->action, temp_state); // Check the requirements of the new action. - if (!is_action_executable(action, preds, funcs)) { + if (!is_action_executable(action, temp_state)) { return false; } } @@ -196,10 +187,8 @@ SimpleBTBuilder::is_parallelizable( return true; } -ActionNode::Ptr -SimpleBTBuilder::get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const ActionGraph::Ptr & graph, +ActionNode::Ptr SimpleBTBuilder::get_node_satisfy( + const plansys2_msgs::msg::Tree & requirement, const ActionGraph::Ptr & graph, const ActionNode::Ptr & current) { ActionNode::Ptr ret; @@ -213,10 +202,8 @@ SimpleBTBuilder::get_node_satisfy( return ret; } -std::list -SimpleBTBuilder::get_node_contradict( - const ActionGraph::Ptr & graph, - const ActionNode::Ptr & current) +std::list SimpleBTBuilder::get_node_contradict( + const ActionGraph::Ptr & graph, const ActionNode::Ptr & current) { std::list ret; @@ -227,11 +214,8 @@ SimpleBTBuilder::get_node_contradict( return ret; } -std::list -SimpleBTBuilder::get_roots( - std::vector & action_sequence, - std::vector & predicates, - std::vector & functions, +std::list SimpleBTBuilder::get_roots( + std::vector & action_sequence, const plansys2::State & state, int & node_counter) { std::list ret; @@ -239,15 +223,12 @@ SimpleBTBuilder::get_roots( auto it = action_sequence.begin(); while (it != action_sequence.end()) { const auto & action = *it; - if (is_action_executable(action, predicates, functions) && - is_parallelizable(action, predicates, functions, ret)) - { + if (is_action_executable(action, state) && is_parallelizable(action, state, ret)) { auto new_root = ActionNode::make_shared(); new_root->action = action; new_root->node_num = node_counter++; new_root->level_num = 0; - new_root->predicates = predicates; - new_root->functions = functions; + new_root->state = state; ret.push_back(new_root); it = action_sequence.erase(it); @@ -259,15 +240,12 @@ SimpleBTBuilder::get_roots( return ret; } -void -SimpleBTBuilder::remove_existing_requirements( - std::vector & requirements, - std::vector & predicates, - std::vector & functions) const +void SimpleBTBuilder::remove_existing_requirements( + std::vector & requirements, const plansys2::State & state) const { auto it = requirements.begin(); while (it != requirements.end()) { - if (check(*it, predicates, functions)) { + if (check(*it, state)) { it = requirements.erase(it); } else { ++it; @@ -275,8 +253,7 @@ SimpleBTBuilder::remove_existing_requirements( } } -void -SimpleBTBuilder::prune_backwards(ActionNode::Ptr new_node, ActionNode::Ptr node_satisfy) +void SimpleBTBuilder::prune_backwards(ActionNode::Ptr new_node, ActionNode::Ptr node_satisfy) { // Repeat prune to the roots for (auto & in : node_satisfy->in_arcs) { @@ -294,8 +271,8 @@ SimpleBTBuilder::prune_backwards(ActionNode::Ptr new_node, ActionNode::Ptr node_ } } -void -SimpleBTBuilder::prune_forward(ActionNode::Ptr current, std::list & used_nodes) +void SimpleBTBuilder::prune_forward( + ActionNode::Ptr current, std::list & used_nodes) { auto it = current->out_arcs.begin(); while (it != current->out_arcs.end()) { @@ -310,39 +287,91 @@ SimpleBTBuilder::prune_forward(ActionNode::Ptr current, std::list & used_nodes, - std::vector & predicates, - std::vector & functions) const +void SimpleBTBuilder::get_state_recursive( + const ActionNode::Ptr & node, std::list & used_nodes, plansys2::State & state) { - // Traverse graph to the root 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); - 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); + get_state_recursive(in, used_nodes, state); // state is modified in-place + apply_action_to_state(in->action, state); // also in-place used_nodes.push_back(in); } } } -ActionGraph::Ptr -SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) +plansys2::State SimpleBTBuilder::get_state( + const ActionNode::Ptr & node, std::list & used_nodes, + const plansys2::State & state) +{ + plansys2::State new_state = state; // Only one copy, at the top. + get_state_recursive(node, used_nodes, new_state); + return new_state; +} + +std::vector SimpleBTBuilder::check_requirements( + const std::vector & requirements, + std::shared_ptr & graph, std::shared_ptr & new_node) +{ + std::vector non_satisfied_requirements; + for (const auto & requirement : requirements) { + if (requirement.nodes[0].node_type == plansys2_msgs::msg::Node::AND) { + auto result = check_requirements(parser::pddl::getSubtrees(requirement), graph, new_node); + non_satisfied_requirements.insert( + std::end(non_satisfied_requirements), std::begin(result), std::end(result)); + } else { + if (!check_requirement(requirement, graph, new_node)) { + non_satisfied_requirements.emplace_back(std::move(requirement)); + } + } + } + return std::move(non_satisfied_requirements); +} + +bool SimpleBTBuilder::check_requirement( + const plansys2_msgs::msg::Tree & requirement, std::shared_ptr & graph, + std::shared_ptr & new_node) +{ + auto parent = get_node_satisfy(requirement, graph, new_node); + if (parent != nullptr) { + prune_backwards(new_node, parent); + + // Create the connections to the parent node + if ( + std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == + new_node->in_arcs.end()) + { + new_node->in_arcs.push_back(parent); + } + if ( + std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == + parent->out_arcs.end()) + { + parent->out_arcs.push_back(new_node); + } + return true; + } + return false; +} + +ActionGraph::Ptr SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) { int node_counter = 0; int level_counter = 0; auto graph = ActionGraph::make_shared(); auto action_sequence = get_plan_actions(current_plan); - auto predicates = problem_client_->getPredicates(); - auto functions = problem_client_->getFunctions(); + + std::vector action_variant_vec; + for (const auto & action : action_sequence) { + action_variant_vec.push_back(action.action); + } + + auto state = problem_client_->getState(); + state.addActionsAndPruneDerived(action_variant_vec); + plansys2::solveDerivedPredicates(state); // Get root actions that can be run in parallel - graph->roots = get_roots(action_sequence, predicates, functions, node_counter); + graph->roots = get_roots(action_sequence, state, node_counter); // Build the rest of the graph while (!action_sequence.empty()) { @@ -362,49 +391,24 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) } new_node->level_num = level_counter; - 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()); + 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( - std::end(requirements), std::begin(at_start_requirements), - std::end(at_start_requirements)); + std::end(requirements), std::begin(at_start_requirements), std::end(at_start_requirements)); requirements.insert( - std::end(requirements), std::begin(over_all_requirements), - std::end(over_all_requirements)); + std::end(requirements), std::begin(over_all_requirements), std::end(over_all_requirements)); requirements.insert( - std::end(requirements), std::begin(at_end_requirements), - std::end(at_end_requirements)); + std::end(requirements), std::begin(at_end_requirements), std::end(at_end_requirements)); // Look for satisfying nodes // A satisfying node is a node with an effect that satisfies a requirement of the new node - auto it = requirements.begin(); - while (it != requirements.end()) { - auto parent = get_node_satisfy(*it, graph, new_node); - if (parent != nullptr) { - prune_backwards(new_node, parent); - - // Create the connections to the parent node - if (std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == - new_node->in_arcs.end()) - { - new_node->in_arcs.push_back(parent); - } - if (std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == - parent->out_arcs.end()) - { - parent->out_arcs.push_back(new_node); - } - - it = requirements.erase(it); - } else { - ++it; - } - } + requirements = check_requirements(requirements, graph, new_node); // Look for contradicting parallel actions // A1 and A2 cannot run in parallel if the effects of A1 contradict the requirements of A2 @@ -413,12 +417,14 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) prune_backwards(new_node, parent); // Create the connections to the parent node - if (std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == + if ( + std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == new_node->in_arcs.end()) { new_node->in_arcs.push_back(parent); } - if (std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == + if ( + std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == parent->out_arcs.end()) { parent->out_arcs.push_back(new_node); @@ -428,18 +434,14 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) // Compute the state up to the new node // The effects of the new node are not applied std::list used_nodes; - predicates = problem_client_->getPredicates(); - functions = problem_client_->getFunctions(); - get_state(new_node, used_nodes, predicates, functions); - new_node->predicates = predicates; - new_node->functions = functions; + new_node->state = get_state(new_node, used_nodes, state); // Check any requirements that do not have satisfying nodes. // These should be satisfied by the initial state. - remove_existing_requirements(requirements, predicates, functions); + remove_existing_requirements(requirements, new_node->state); for (const auto & req : requirements) { - std::cerr << "[ERROR] requirement not met: [" << - parser::pddl::toString(req) << "]" << std::endl; + std::cerr << "[ERROR] requirement not met: [" << parser::pddl::toString(req) << "]" + << std::endl; } // Return and empyt graph to fail the serveice call @@ -453,8 +455,7 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) return graph; } -std::string -SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) +std::string SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) { graph_ = get_graph(current_plan); @@ -472,20 +473,18 @@ SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) std::list used_nodes; if (graph_->roots.size() > 1) { - bt_ = std::string("\n") + - t(1) + "\n" + - t(2) + "roots.size()) + - "\" failure_count=\"1\">\n"; + bt_ = std::string("\n") + t(1) + + "\n" + t(2) + "roots.size()) + "\" failure_count=\"1\">\n"; for (const auto & node : graph_->roots) { bt_ = bt_ + get_flow_tree(node, used_nodes, 3); } - bt_ = bt_ + t(2) + "\n" + - t(1) + "\n\n"; + bt_ = bt_ + t(2) + "\n" + t(1) + "\n\n"; } else { - bt_ = std::string("\n") + - t(1) + "\n"; + bt_ = std::string("\n") + t(1) + + "\n"; bt_ = bt_ + get_flow_tree(*graph_->roots.begin(), used_nodes, 2); @@ -495,10 +494,8 @@ SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) return bt_; } -std::string -SimpleBTBuilder::get_dotgraph( - std::shared_ptr> action_map, - bool enable_legend, +std::string SimpleBTBuilder::get_dotgraph( + std::shared_ptr> action_map, bool enable_legend, bool enable_print_graph) { if (enable_print_graph) { @@ -603,17 +600,14 @@ SimpleBTBuilder::get_dotgraph( return ss.str(); } -std::string -SimpleBTBuilder::get_flow_tree( - ActionNode::Ptr node, - std::list & used_nodes, - int level) +std::string SimpleBTBuilder::get_flow_tree( + ActionNode::Ptr node, std::list & used_nodes, int level) { std::string ret; int l = level; - const std::string action_id = "(" + node->action.action.get_action_string() + "):" + - std::to_string(static_cast(node->action.time * 1000)); + 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()) { return t(l) + "\n"; @@ -636,8 +630,7 @@ SimpleBTBuilder::get_flow_tree( ret = ret + t(l) + "\n"; ret = ret + execution_block(node, l + 1); - ret = ret + t(l + 1) + - "out_arcs.size()) + + ret = ret + t(l + 1) + "out_arcs.size()) + "\" failure_count=\"1\">\n"; for (const auto & child_node : node->out_arcs) { @@ -651,28 +644,23 @@ SimpleBTBuilder::get_flow_tree( return ret; } -void -SimpleBTBuilder::get_flow_dotgraph( - ActionNode::Ptr node, - std::set & edges) +void SimpleBTBuilder::get_flow_dotgraph(ActionNode::Ptr node, std::set & edges) { for (const auto & arc : node->out_arcs) { - std::string edge = std::to_string(node->node_num) + "->" + std::to_string(arc->node_num) + - ";\n"; + std::string edge = + std::to_string(node->node_num) + "->" + std::to_string(arc->node_num) + ";\n"; edges.insert(edge); get_flow_dotgraph(arc, edges); } } -std::string -SimpleBTBuilder::get_node_dotgraph( - ActionNode::Ptr node, std::shared_ptr> action_map, int level) +std::string SimpleBTBuilder::get_node_dotgraph( + ActionNode::Ptr node, std::shared_ptr> action_map, + int level) { std::stringstream ss; ss << t(level); - ss << node->node_num << " [label=\"" << node->action.action.get_action_string() << - "\""; + ss << node->node_num << " [label=\"" << node->action.action.get_action_string() << "\""; ss << "labeljust=c,style=filled"; auto status = get_action_status(node->action, action_map); @@ -703,8 +691,8 @@ ActionExecutor::Status SimpleBTBuilder::get_action_status( ActionStamped action_stamped, std::shared_ptr> action_map) { - auto index = "(" + action_stamped.action.get_action_string() + "):" + - std::to_string(static_cast(action_stamped.time * 1000)); + 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(); } else { @@ -713,8 +701,7 @@ ActionExecutor::Status SimpleBTBuilder::get_action_status( } void SimpleBTBuilder::addDotGraphLegend( - std::stringstream & ss, int tab_level, int level_counter, - int node_counter) + std::stringstream & ss, int tab_level, int level_counter, int node_counter) { int legend_counter = level_counter; int legend_node_counter = node_counter; @@ -738,17 +725,18 @@ void SimpleBTBuilder::addDotGraphLegend( ss << t(tab_level); ss << "labeljust = l;\n"; ss << t(tab_level); - ss << legend_node_counter++ << - " [label=\n\"Finished action\n\",labeljust=c,style=filled,color=green4,fillcolor=seagreen2];\n"; + ss << legend_node_counter++ + << " [label=\n\"Finished " + "action\n\",labeljust=c,style=filled,color=green4,fillcolor=seagreen2];\n"; ss << t(tab_level); - ss << legend_node_counter++ << - " [label=\n\"Failed action\n\",labeljust=c,style=filled,color=red,fillcolor=pink];\n"; + ss << legend_node_counter++ + << " [label=\n\"Failed action\n\",labeljust=c,style=filled,color=red,fillcolor=pink];\n"; ss << t(tab_level); - ss << legend_node_counter++ << - " [label=\n\"Current action\n\",labeljust=c,style=filled,color=blue,fillcolor=skyblue];\n"; + ss << legend_node_counter++ + << " [label=\n\"Current action\n\",labeljust=c,style=filled,color=blue,fillcolor=skyblue];\n"; ss << t(tab_level); - ss << legend_node_counter++ << " [label=\n\"Future action\n\",labeljust=c,style=filled," << - "color=yellow3,fillcolor=lightgoldenrod1];\n"; + ss << legend_node_counter++ << " [label=\n\"Future action\n\",labeljust=c,style=filled," + << "color=yellow3,fillcolor=lightgoldenrod1];\n"; tab_level--; ss << t(tab_level); ss << "}\n"; @@ -767,8 +755,7 @@ void SimpleBTBuilder::addDotGraphLegend( ss << "}\n"; } -std::string -SimpleBTBuilder::t(int level) +std::string SimpleBTBuilder::t(int level) { std::string ret; for (int i = 0; i < level; i++) { @@ -777,21 +764,20 @@ SimpleBTBuilder::t(int level) return ret; } -std::string -SimpleBTBuilder::execution_block(const ActionNode::Ptr & node, int l) +std::string 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 = "(" + node->action.action.get_action_string() + "):" + - std::to_string(static_cast(action.time * 1000)); + 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 = "(" + - previous_node->action.action.get_action_string() + "):" + - std::to_string(static_cast( previous_node->action.time * 1000)); + const std::string parent_action_id = + "(" + previous_node->action.action.get_action_string() + + "):" + std::to_string(static_cast(previous_node->action.time * 1000)); wait_actions = wait_actions + t(1) + ""; if (previous_node != *node->in_arcs.rbegin()) { @@ -812,8 +798,7 @@ SimpleBTBuilder::execution_block(const ActionNode::Ptr & node, int l) return ret; } -std::vector -SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) +std::vector SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) { std::vector ret; @@ -825,11 +810,9 @@ SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) 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)); + domain_client_->getAction(get_action_name(item.action), get_action_params(item.action)); } else { - action_stamped.action = - domain_client_->getDurativeAction( + action_stamped.action = domain_client_->getDurativeAction( get_action_name(item.action), get_action_params(item.action)); } @@ -839,10 +822,8 @@ SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) return ret; } -void -SimpleBTBuilder::print_node( - const plansys2::ActionNode::Ptr & node, - int level, +void SimpleBTBuilder::print_node( + const plansys2::ActionNode::Ptr & node, int level, std::set & used_nodes) const { std::cerr << std::string(level, '\t') << "[" << node->action.time << "] "; @@ -858,8 +839,7 @@ SimpleBTBuilder::print_node( } } -void -SimpleBTBuilder::print_graph(const plansys2::ActionGraph::Ptr & graph) const +void SimpleBTBuilder::print_graph(const plansys2::ActionGraph::Ptr & graph) const { std::set used_nodes; for (const auto & root : graph->roots) { @@ -867,11 +847,10 @@ SimpleBTBuilder::print_graph(const plansys2::ActionGraph::Ptr & graph) const } } -void -SimpleBTBuilder::print_node_csv(const plansys2::ActionNode::Ptr & node, uint32_t root_num) const +void SimpleBTBuilder::print_node_csv( + const plansys2::ActionNode::Ptr & node, uint32_t root_num) const { - std::string out_str = std::to_string(root_num) + ", " + - std::to_string(node->node_num) + ", " + + std::string out_str = std::to_string(root_num) + ", " + std::to_string(node->node_num) + ", " + std::to_string(node->level_num) + ", " + node->action.action.get_action_string(); for (const auto & arc : node->out_arcs) { @@ -883,8 +862,7 @@ SimpleBTBuilder::print_node_csv(const plansys2::ActionNode::Ptr & node, uint32_t } } -void -SimpleBTBuilder::print_graph_csv(const plansys2::ActionGraph::Ptr & graph) const +void SimpleBTBuilder::print_graph_csv(const plansys2::ActionGraph::Ptr & graph) const { uint32_t root_num = 0; for (const auto & root : graph->roots) { @@ -893,16 +871,13 @@ SimpleBTBuilder::print_graph_csv(const plansys2::ActionGraph::Ptr & graph) const } } -void -SimpleBTBuilder::get_node_tabular( - const plansys2::ActionNode::Ptr & node, - uint32_t root_num, +void SimpleBTBuilder::get_node_tabular( + const plansys2::ActionNode::Ptr & node, uint32_t root_num, std::vector> & graph) const { graph.push_back( std::make_tuple( - root_num, node->node_num, node->level_num, - node->action.action.get_action_string())); + root_num, node->node_num, node->level_num, 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 de6ebfa8e..1b8b987f8 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 @@ -260,11 +260,10 @@ STNBTBuilder::init_graph(const plansys2_msgs::msg::Plan & plan) const auto action_sequence = get_plan_actions(plan); // Add a node to represent the initial state - auto predicates = problem_client_->getPredicates(); - auto functions = problem_client_->getFunctions(); + auto state = problem_client_->getState(); auto init_action = std::make_shared(); - init_action->at_end_effects = from_state(predicates, functions); + init_action->at_end_effects = from_state(state); int node_cnt = 0; auto init_node = Node::make_shared(node_cnt++); @@ -391,11 +390,10 @@ STNBTBuilder::get_simple_plan(const plansys2_msgs::msg::Plan & plan) const auto action_sequence = get_plan_actions(plan); // Add an action to represent the initial state - auto predicates = problem_client_->getPredicates(); - auto functions = problem_client_->getFunctions(); + auto state = problem_client_->getState(); auto init_action_ = std::make_shared(); - init_action_->at_end_effects = from_state(predicates, functions); + init_action_->at_end_effects = from_state(state); ActionStamped init_action; init_action.action = init_action_; init_action.type = ActionType::INIT; @@ -471,30 +469,28 @@ STNBTBuilder::get_simple_plan(const plansys2_msgs::msg::Plan & plan) const return simple_plan; } -std::map +std::map STNBTBuilder::get_states( const std::set & happenings, const std::multimap & plan) const { - std::map states; + std::map states; - StateVec state_vec; - state_vec.predicates = problem_client_->getPredicates(); - state_vec.functions = problem_client_->getFunctions(); - states.insert(std::make_pair(-1, state_vec)); + auto state = problem_client_->getState(); + states.insert(std::make_pair(-1, state)); for (const auto & time : happenings) { 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.get_at_start_effects(), state_vec.predicates, state_vec.functions); + plansys2::apply( + iter->second.action.get_at_start_effects(), state); } else if (iter->second.type == ActionType::END) { - apply( - iter->second.action.get_at_end_effects(), state_vec.predicates, state_vec.functions); + plansys2::apply( + iter->second.action.get_at_end_effects(), state); } } - states.insert(std::make_pair(time, state_vec)); + states.insert(std::make_pair(time, state)); } return states; @@ -502,8 +498,7 @@ STNBTBuilder::get_states( plansys2_msgs::msg::Tree STNBTBuilder::from_state( - const std::vector & preds, - const std::vector & funcs) const + const plansys2::State & state) const { plansys2_msgs::msg::Tree tree; plansys2_msgs::msg::Node node; @@ -512,14 +507,14 @@ STNBTBuilder::from_state( node.negate = false; tree.nodes.push_back(node); - for (const auto & pred : preds) { + for (const auto & pred : state.getPredicates()) { const plansys2_msgs::msg::Node * child = &pred; tree.nodes.push_back(*child); tree.nodes.back().node_id = tree.nodes.size() - 1; tree.nodes[0].children.push_back(tree.nodes.size() - 1); } - for (const auto & func : funcs) { + for (const auto & func : state.getFunctions()) { const plansys2_msgs::msg::Node * child = &func; tree.nodes.push_back(*child); tree.nodes.back().node_id = tree.nodes.size() - 1; @@ -613,7 +608,7 @@ STNBTBuilder::get_parents( const std::pair & action, const std::multimap & plan, const std::set & happenings, - const std::map & states) const + const std::map & states) const { auto parents = get_satisfy(action, plan, happenings, states); auto threats = get_threat(action, plan, happenings, states); @@ -627,7 +622,7 @@ STNBTBuilder::get_satisfy( const std::pair & action, const std::multimap & plan, const std::set & happenings, - const std::map & states) const + const std::map & states) const { std::vector> ret; @@ -656,7 +651,7 @@ STNBTBuilder::get_satisfy( auto X_1 = X_it->second; for (const auto & r : R_a) { - if (!check(r, X_1.predicates, X_1.functions)) { + if (!check(r, X_1)) { auto it = plan.equal_range(t_2); for (auto iter = it.first; iter != it.second; ++iter) { if (iter->first == action.first) { @@ -668,10 +663,10 @@ STNBTBuilder::get_satisfy( auto E_k = get_effects(iter->second); auto X_hat = X_1; - apply(E_k, X_hat.predicates, X_hat.functions); + plansys2::apply(E_k, X_hat); // Check if action k satisfies action i - if (check(r, X_hat.predicates, X_hat.functions)) { + if (check(r, X_hat)) { ret.push_back(*iter); } } @@ -680,11 +675,10 @@ STNBTBuilder::get_satisfy( t_2 = t_1; } - auto predicates = problem_client_->getPredicates(); - auto functions = problem_client_->getFunctions(); + auto state = problem_client_->getState(); for (const auto & r : R_a) { - if (check(r, predicates, functions)) { + if (check(r, state)) { ret.push_back(*plan.begin()); } } @@ -697,7 +691,7 @@ STNBTBuilder::get_threat( const std::pair & action, const std::multimap & plan, const std::set & happenings, - const std::map & states) const + const std::map & states) const { std::vector> ret; @@ -753,11 +747,11 @@ STNBTBuilder::get_threat( auto E_k = get_effects(iter->second); auto X_hat = X_1_k; - apply(E_a, X_hat.predicates, X_hat.functions); + plansys2::apply(E_a, X_hat); // Check if the input action threatens action k if (action.second.type != ActionType::OVERALL && - !check(R_k, X_hat.predicates, X_hat.functions)) + !check(R_k, X_hat)) { if (t_2 != t_in) { ret.push_back(*iter); @@ -770,11 +764,11 @@ STNBTBuilder::get_threat( } auto X_bar = X_1_a; - apply(E_k, X_bar.predicates, X_bar.functions); + plansys2::apply(E_k, X_bar); // Check if action k threatens the input action if (iter->second.type != ActionType::OVERALL && - !check(R_a, X_bar.predicates, X_bar.functions)) + !check(R_a, X_bar)) { if (t_2 != t_in) { ret.push_back(*iter); @@ -793,7 +787,7 @@ STNBTBuilder::get_threat( auto DX_hat = get_diff(X_1_k, X_hat); auto DX_bar = get_diff(X_1_a, X_bar); auto intersection = get_intersection(DX_hat, DX_bar); - if (intersection.predicates.size() > 0 || intersection.functions.size() > 0) { + if (intersection.getPredicatesSize() > 0 || intersection.getFunctionsSize() > 0) { if (t_2 != t_in) { ret.push_back(*iter); } else { @@ -816,12 +810,12 @@ STNBTBuilder::can_apply( const std::pair & action, const std::multimap & plan, const int & time, - StateVec & state) const + plansys2::State & state) const { auto X = state; auto R = get_conditions(action.second); - if (check(R, X.predicates, X.functions)) { + if (check(R, X)) { return true; } @@ -842,8 +836,8 @@ STNBTBuilder::can_apply( } } auto E = get_effects(iter->second); - apply(E, state.predicates, state.functions); - if (check(R, state.predicates, state.functions)) { + plansys2::apply(E, state); + if (check(R, state)) { return true; } } @@ -854,49 +848,34 @@ STNBTBuilder::can_apply( return false; } -StateVec +plansys2::State STNBTBuilder::get_diff( - const StateVec & X_1, - const StateVec & X_2) const + const plansys2::State & X_1, + const plansys2::State & X_2) const { - StateVec ret; + plansys2::State ret; // Look for predicates in X_1 that are not in X_2 - for (const auto & p_1 : X_1.predicates) { - auto it = std::find_if( - X_2.predicates.begin(), X_2.predicates.end(), - [&](plansys2::Predicate p_2) { - return parser::pddl::checkNodeEquality(p_1, p_2); - }); - if (it == X_2.predicates.end()) { - ret.predicates.push_back(p_1); + for (const auto & p_1 : X_1.getPredicates()) { + if(!X_2.hasPredicate(p_1)) { + ret.addPredicate(p_1); } } // Look for predicates in X_2 that are not in X_1 - for (const auto & p_2 : X_2.predicates) { - auto it = std::find_if( - X_1.predicates.begin(), X_1.predicates.end(), - [&](plansys2::Predicate p_1) { - return parser::pddl::checkNodeEquality(p_1, p_2); - }); - if (it == X_1.predicates.end()) { - ret.predicates.push_back(p_2); + for (const auto & p_2 : X_2.getPredicates()) { + if(!X_1.hasPredicate(p_2)) { + ret.addPredicate(p_2); } } // Look for function changes - for (const auto & f_1 : X_1.functions) { - auto it = std::find_if( - X_2.functions.begin(), X_2.functions.end(), - [&](plansys2::Function f_2) { - return parser::pddl::checkNodeEquality(f_1, f_2); - }); - if (it != X_2.functions.end()) { - if (std::abs(f_1.value - it->value) > - 1e-5 * std::max(std::abs(f_1.value), std::abs(it->value))) + for (const auto & f_1 : X_1.getFunctions()) { + if (X_2.hasFunction(f_1)) { + if (std::abs(f_1.value - X_2.getFunction(f_1)->value) > + 1e-5 * std::max(std::abs(f_1.value), std::abs(X_2.getFunction(f_1)->value))) { - ret.functions.push_back(f_1); + ret.addFunction(f_1); } } } @@ -904,34 +883,24 @@ STNBTBuilder::get_diff( return ret; } -StateVec +plansys2::State STNBTBuilder::get_intersection( - const StateVec & X_1, - const StateVec & X_2) const + const plansys2::State & X_1, + const plansys2::State & X_2) const { - StateVec ret; + plansys2::State ret; // Look for predicates in X_1 that are also in X_2 - for (const auto & p_1 : X_1.predicates) { - auto it = std::find_if( - X_2.predicates.begin(), X_2.predicates.end(), - [&](plansys2::Predicate p_2) { - return parser::pddl::checkNodeEquality(p_1, p_2); - }); - if (it != X_2.predicates.end()) { - ret.predicates.push_back(p_1); + for (const auto & p_1 : X_1.getPredicates()) { + if (X_2.hasPredicate(p_1)) { + ret.addPredicate(p_1); } } // Look for functions in X_1 that are also in X_2 - for (const auto & f_1 : X_1.functions) { - auto it = std::find_if( - X_2.functions.begin(), X_2.functions.end(), - [&](plansys2::Function f_2) { - return parser::pddl::checkNodeEquality(f_1, f_2); - }); - if (it != X_2.functions.end()) { - ret.functions.push_back(f_1); + for (const auto & f_1 : X_1.getFunctions()) { + if (X_2.hasFunction(f_1)) { + ret.addFunction(f_1); } } diff --git a/plansys2_executor/test/unit/CMakeLists.txt b/plansys2_executor/test/unit/CMakeLists.txt index 9535c1f2f..b06039aac 100644 --- a/plansys2_executor/test/unit/CMakeLists.txt +++ b/plansys2_executor/test/unit/CMakeLists.txt @@ -19,7 +19,7 @@ target_link_libraries(execution_tree_test ${PROJECT_NAME} ) -ament_add_gtest(simple_btbuilder_tests simple_btbuilder_tests.cpp) +ament_add_gtest(simple_btbuilder_tests simple_btbuilder_tests.cpp TIMEOUT 1200) target_compile_definitions(simple_btbuilder_tests PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") target_link_libraries(simple_btbuilder_tests @@ -48,3 +48,8 @@ target_compile_definitions(bt_node_test_charging target_link_libraries(bt_node_test_charging ${PROJECT_NAME} ) + +ament_add_gtest(bt_node_test_suave bt_node_test_suave.cpp) +target_compile_definitions(bt_node_test_suave + PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_link_libraries(bt_node_test_suave ${PROJECT_NAME}) diff --git a/plansys2_executor/test/unit/action_execution_test.cpp b/plansys2_executor/test/unit/action_execution_test.cpp index 902776c4f..d2aee6457 100644 --- a/plansys2_executor/test/unit/action_execution_test.cpp +++ b/plansys2_executor/test/unit/action_execution_test.cpp @@ -12,54 +12,45 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - -#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/blackboard.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" - +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/BTBuilder.hpp" #include "plansys2_executor/ExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - -#include "behaviortree_cpp/behavior_tree.h" -#include "behaviortree_cpp/bt_factory.h" -#include "behaviortree_cpp/utils/shared_library.h" -#include "behaviortree_cpp/blackboard.h" - +#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" - -#include "lifecycle_msgs/msg/state.hpp" - +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - - -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using namespace std::chrono_literals; class MoveAction : public plansys2::ActionExecutorClient @@ -79,8 +70,7 @@ class MoveAction : public plansys2::ActionExecutorClient cycles_ = 0; } - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "MoveAction::on_activate" << std::endl; counter_ = 0; @@ -142,7 +132,9 @@ TEST(action_execution, protocol_basic) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::IDLE); @@ -162,8 +154,7 @@ TEST(action_execution, protocol_basic) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( move_action_node->get_internal_status().state, @@ -208,7 +199,6 @@ TEST(action_execution, protocol_basic) move_action_node->get_internal_status().state, plansys2_msgs::msg::ActionPerformerStatus::READY); - ASSERT_EQ(action_execution_msgs.size(), 8u); ASSERT_EQ(action_execution_msgs[3].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); ASSERT_EQ(action_execution_msgs[4].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); @@ -216,7 +206,6 @@ TEST(action_execution, protocol_basic) ASSERT_EQ(action_execution_msgs[6].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); ASSERT_EQ(action_execution_msgs[7].type, plansys2_msgs::msg::ActionExecution::FINISH); - ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::SUCCESS); ASSERT_EQ( move_action_node->get_internal_status().state, @@ -258,7 +247,9 @@ TEST(action_execution, protocol_cancelation) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::IDLE); @@ -278,8 +269,7 @@ TEST(action_execution, protocol_cancelation) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( move_action_node->get_internal_status().state, @@ -300,8 +290,7 @@ TEST(action_execution, protocol_cancelation) ASSERT_EQ( move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ( - move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::RUNNING); + ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::RUNNING); ASSERT_EQ( move_action_node->get_internal_status().state, plansys2_msgs::msg::ActionPerformerStatus::RUNNING); @@ -332,13 +321,11 @@ TEST(action_execution, protocol_cancelation) } ASSERT_EQ( - move_action_executor->get_internal_status(), - plansys2::ActionExecutor::Status::CANCELLED); + move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::CANCELLED); ASSERT_EQ( move_action_node->get_internal_status().state, plansys2_msgs::msg::ActionPerformerStatus::READY); - ASSERT_EQ(action_execution_msgs.size(), 6u); ASSERT_EQ(action_execution_msgs[3].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); ASSERT_EQ(action_execution_msgs[4].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); diff --git a/plansys2_executor/test/unit/bt_node_test.cpp b/plansys2_executor/test/unit/bt_node_test.cpp index e1be64377..434b8c50e 100644 --- a/plansys2_executor/test/unit/bt_node_test.cpp +++ b/plansys2_executor/test/unit/bt_node_test.cpp @@ -12,52 +12,53 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" -#include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" - -#include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" -#include "plansys2_pddl_parser/Utils.hpp" - #include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/blackboard.h" #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/utils/shared_library.h" -#include "behaviortree_cpp/blackboard.h" +#include "gtest/gtest.h" + +#include "lifecycle_msgs/msg/state.hpp" + +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" + +#include "plansys2_executor/ActionExecutor.hpp" +#include "plansys2_executor/ActionExecutorClient.hpp" +#include "plansys2_executor/BTBuilder.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" #include "plansys2_executor/behavior_tree/restore_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" -#include "lifecycle_msgs/msg/state.hpp" +#include "plansys2_pddl_parser/Utils.hpp" + +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" + +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - - TEST(problem_expert, wait_overall_req_test) { auto test_node = rclcpp::Node::make_shared("test_node"); @@ -80,10 +81,11 @@ TEST(problem_expert, wait_overall_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -137,15 +139,13 @@ TEST(problem_expert, wait_overall_req_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("CheckOverAllReq"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -193,10 +193,11 @@ TEST(problem_expert, wait_atstart_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -250,15 +251,13 @@ TEST(problem_expert, wait_atstart_req_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("WaitAtStartReq"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -271,7 +270,6 @@ TEST(problem_expert, wait_atstart_req_test) status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); - for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } @@ -308,10 +306,11 @@ TEST(problem_expert, wait_atend_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -365,15 +364,13 @@ TEST(problem_expert, wait_atend_req_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("CheckAtEndReq"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -421,10 +418,11 @@ TEST(problem_expert, at_start_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -478,7 +476,6 @@ TEST(problem_expert, at_start_effect_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("ApplyAtStartEffect"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); @@ -486,8 +483,7 @@ TEST(problem_expert, at_start_effect_test) try { std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); @@ -506,9 +502,7 @@ TEST(problem_expert, at_start_effect_test) } } ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at robot1 wheels_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at robot1 wheels_zone)"))); } catch (std::exception & e) { std::cerr << e.what() << std::endl; } @@ -659,10 +653,11 @@ TEST(problem_expert, at_end_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -716,15 +711,13 @@ TEST(problem_expert, at_end_effect_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("ApplyAtEndEffect"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); try { - std::vector predicates = { - "(robot_at robot1 wheels_zone)"}; + std::vector predicates = {"(robot_at robot1 wheels_zone)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); @@ -744,9 +737,7 @@ TEST(problem_expert, at_end_effect_test) } ASSERT_TRUE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at robot1 assembly_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at robot1 assembly_zone)"))); } catch (std::exception & e) { std::cerr << e.what() << std::endl; } diff --git a/plansys2_executor/test/unit/bt_node_test_charging.cpp b/plansys2_executor/test/unit/bt_node_test_charging.cpp index 97cf57ec2..18bdc1c3f 100644 --- a/plansys2_executor/test/unit/bt_node_test_charging.cpp +++ b/plansys2_executor/test/unit/bt_node_test_charging.cpp @@ -12,47 +12,48 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" -#include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" - -#include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/blackboard.h" #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/utils/shared_library.h" -#include "behaviortree_cpp/blackboard.h" +#include "gtest/gtest.h" + +#include "lifecycle_msgs/msg/state.hpp" + +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" + +#include "plansys2_executor/ActionExecutor.hpp" +#include "plansys2_executor/ActionExecutorClient.hpp" +#include "plansys2_executor/BTBuilder.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" #include "plansys2_executor/behavior_tree/restore_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" -#include "lifecycle_msgs/msg/state.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" + +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - TEST(problem_expert, wait_atstart_req_test) { auto test_node = rclcpp::Node::make_shared("test_node"); @@ -75,10 +76,11 @@ TEST(problem_expert, wait_atstart_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -103,8 +105,7 @@ 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"].action_info = - domain_client->getDurativeAction( + (*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)")); @@ -136,16 +137,11 @@ TEST(problem_expert, wait_atstart_req_test) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wp2", "waypoint"))); std::vector predicates = { - "(robot_at robot1 wp1)", - "(charger_at wp2)", - "(connected wp1 wp2)"}; + "(robot_at robot1 wp1)", "(charger_at wp2)", "(connected wp1 wp2)"}; std::vector functions = { - "(= (speed robot1) 3)", - "(= (max_range robot1) 75)", - "(= (state_of_charge robot1) 99)", - "(= (distance wp1 wp2) 15)", - "(= (distance wp2 wp1) 15)"}; + "(= (speed robot1) 3)", "(= (max_range robot1) 75)", "(= (state_of_charge robot1) 99)", + "(= (distance wp1 wp2) 15)", "(= (distance wp2 wp1) 15)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -198,10 +194,11 @@ TEST(problem_expert, apply_atstart_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -226,8 +223,7 @@ 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"].action_info = - domain_client->getDurativeAction( + (*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)")); @@ -261,20 +257,15 @@ TEST(problem_expert, apply_atstart_effect_test) try { std::vector predicates = { - "(robot_at robot1 wp1)", - "(charger_at wp2)", - "(connected wp1 wp2)"}; + "(robot_at robot1 wp1)", "(charger_at wp2)", "(connected wp1 wp2)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } std::vector functions = { - "(= (speed robot1) 3)", - "(= (max_range robot1) 75)", - "(= (state_of_charge robot1) 99)", - "(= (distance wp1 wp2) 15)", - "(= (distance wp2 wp1) 15)"}; + "(= (speed robot1) 3)", "(= (max_range robot1) 75)", "(= (state_of_charge robot1) 99)", + "(= (distance wp1 wp2) 15)", "(= (distance wp2 wp1) 15)"}; for (const auto & func : functions) { ASSERT_TRUE(problem_client->addFunction(plansys2::Function(func))); @@ -451,10 +442,11 @@ TEST(problem_expert, apply_atend_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -479,8 +471,7 @@ 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"].action_info = - domain_client->getDurativeAction( + (*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)")); @@ -513,20 +504,15 @@ TEST(problem_expert, apply_atend_effect_test) try { std::vector predicates = { - "(robot_at robot1 wp1)", - "(charger_at wp2)", - "(connected wp1 wp2)"}; + "(robot_at robot1 wp1)", "(charger_at wp2)", "(connected wp1 wp2)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } std::vector functions = { - "(= (speed robot1) 3)", - "(= (max_range robot1) 75)", - "(= (state_of_charge robot1) 99)", - "(= (distance wp1 wp2) 15)", - "(= (distance wp2 wp1) 15)"}; + "(= (speed robot1) 3)", "(= (max_range robot1) 75)", "(= (state_of_charge robot1) 99)", + "(= (distance wp1 wp2) 15)", "(= (distance wp2 wp1) 15)"}; for (const auto & func : functions) { ASSERT_TRUE(problem_client->addFunction(plansys2::Function(func))); diff --git a/plansys2_executor/test/unit/bt_node_test_suave.cpp b/plansys2_executor/test/unit/bt_node_test_suave.cpp new file mode 100644 index 000000000..87cb324a9 --- /dev/null +++ b/plansys2_executor/test/unit/bt_node_test_suave.cpp @@ -0,0 +1,488 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/blackboard.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "plansys2_executor/ActionExecutor.hpp" +#include "plansys2_executor/ActionExecutorClient.hpp" +#include "plansys2_executor/BTBuilder.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" +#include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class ValidateDomainNode : public rclcpp::Node +{ +public: + ValidateDomainNode() + : Node("validate_domain_server") + { + service_ = this->create_service( + "planner/validate_domain", std::bind( + &ValidateDomainNode::handle_validate_domain, this, + std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "Service 'planner/validate_domain' is ready."); + } + +private: + void handle_validate_domain( + const std::shared_ptr/* request */, + std::shared_ptr response) + { + response->success = true; + RCLCPP_INFO(this->get_logger(), "Handled validate_domain request: returning true"); + } + + rclcpp::Service::SharedPtr service_; +}; + +TEST(bt_node_test_suave, suave_bt_execution_test) +{ + auto test_node = rclcpp::Node::make_shared("test_node"); + auto test_lc_node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); + auto validate_domain_node = std::make_shared(); + auto domain_node = std::make_shared(); + auto problem_node = std::make_shared(); + + auto domain_client = std::make_shared(); + auto problem_client = std::make_shared(); + + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_executor"); + + domain_node->set_parameter({"model_file", pkgpath + "/pddl/suave_domain.pddl"}); + domain_node->set_parameter({"validate_using_planner_node", true}); + problem_node->set_parameter({"model_file", pkgpath + "/pddl/suave_domain.pddl"}); + problem_node->set_parameter({"problem_file", pkgpath + "/pddl/suave_problem.pddl"}); + + rclcpp::executors::MultiThreadedExecutor exe(rclcpp::ExecutorOptions(), 8); + + exe.add_node(validate_domain_node->get_node_base_interface()); + exe.add_node(domain_node->get_node_base_interface()); + exe.add_node(problem_node->get_node_base_interface()); + + bool finish = false; + std::thread t([&]() { + while (!finish) { + exe.spin_some(); + } + }); + + domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + auto action_map = std::make_shared>(); + (*action_map)["(start_robot bluerov):0"] = plansys2::ActionExecutionInfo(); + (*action_map)["(start_robot bluerov):0"].action_info = domain_client->getAction( + plansys2::get_action_name("(start_robot bluerov)"), + plansys2::get_action_params("(start_robot bluerov)")); + + (*action_map)["(reconfigure1 f_maintain_motion fd_all_thrusters):0"] = + plansys2::ActionExecutionInfo(); + (*action_map)["(reconfigure1 f_maintain_motion fd_all_thrusters):0"].action_info = + domain_client->getAction( + plansys2::get_action_name("(reconfigure1 f_maintain_motion fd_all_thrusters)"), + plansys2::get_action_params("(reconfigure1 f_maintain_motion fd_all_thrusters)")); + + (*action_map)["(reconfigure1 f_generate_search_path fd_spiral_high):0"] = + plansys2::ActionExecutionInfo(); + (*action_map)["(reconfigure1 f_generate_search_path fd_spiral_high):0"].action_info = + domain_client->getAction( + plansys2::get_action_name("(reconfigure1 f_generate_search_path fd_spiral_high)"), + plansys2::get_action_params("(reconfigure1 f_generate_search_path fd_spiral_high)")); + + (*action_map)["(search_pipeline pipeline bluerov):0"] = plansys2::ActionExecutionInfo(); + (*action_map)["(search_pipeline pipeline bluerov):0"].action_info = domain_client->getAction( + plansys2::get_action_name("(search_pipeline pipeline bluerov)"), + plansys2::get_action_params("(search_pipeline pipeline bluerov)")); + + (*action_map)["(reconfigure1 f_follow_pipeline fd_follow_pipeline):0"] = + plansys2::ActionExecutionInfo(); + (*action_map)["(reconfigure1 f_follow_pipeline fd_follow_pipeline):0"].action_info = + domain_client->getAction( + plansys2::get_action_name("(reconfigure1 f_follow_pipeline fd_follow_pipeline)"), + plansys2::get_action_params("(reconfigure1 f_follow_pipeline fd_follow_pipeline)")); + + (*action_map)["(reconfigure2 f_generate_search_path fd_spiral_high fd_unground):0"] = + plansys2::ActionExecutionInfo(); + (*action_map)["(reconfigure2 f_generate_search_path fd_spiral_high fd_unground):0"].action_info = + domain_client->getAction( + plansys2::get_action_name("(reconfigure2 f_generate_search_path fd_spiral_high fd_unground)"), + plansys2::get_action_params( + "(reconfigure2 f_generate_search_path fd_spiral_high fd_unground)")); + + (*action_map)["(inspect_pipeline pipeline bluerov):0"] = plansys2::ActionExecutionInfo(); + (*action_map)["(inspect_pipeline pipeline bluerov):0"].action_info = domain_client->getAction( + plansys2::get_action_name("(inspect_pipeline pipeline bluerov)"), + plansys2::get_action_params("(inspect_pipeline pipeline bluerov)")); + + ASSERT_FALSE((*action_map)["(start_robot bluerov):0"].action_info.is_empty()); + ASSERT_FALSE( + (*action_map)["(reconfigure1 f_maintain_motion fd_all_thrusters):0"].action_info.is_empty()); + ASSERT_FALSE( + (*action_map)["(reconfigure1 f_generate_search_path fd_spiral_high):0"].action_info.is_empty()); + ASSERT_FALSE((*action_map)["(search_pipeline pipeline bluerov):0"].action_info.is_empty()); + ASSERT_FALSE( + (*action_map)["(reconfigure1 f_follow_pipeline fd_follow_pipeline):0"].action_info.is_empty()); + ASSERT_FALSE( + (*action_map)["(reconfigure2 f_generate_search_path fd_spiral_high fd_unground):0"] + .action_info.is_empty()); + ASSERT_FALSE((*action_map)["(inspect_pipeline pipeline bluerov):0"].action_info.is_empty()); + + std::string bt_xml_tree = + R"( + + + + + + + + + + )"; + + auto blackboard = BT::Blackboard::create(); + + blackboard->set("action_map", action_map); + blackboard->set("node", test_lc_node); + blackboard->set("problem_client", problem_client); + + BT::BehaviorTreeFactory factory; + factory.registerNodeType("ExecuteAction"); + factory.registerNodeType("ApplyAtEndEffect"); + factory.registerNodeType("ApplyAtStartEffect"); + factory.registerNodeType("CheckOverAllReq"); + factory.registerNodeType("CheckAtEndReq"); + + try { + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + status = tree.tickOnce(); + ASSERT_EQ(status, BT::NodeStatus::SUCCESS); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + auto state = problem_client->getState(); + state.addActionsAndPruneDerived( + {(*action_map)["(search_pipeline pipeline bluerov):0"].action_info}); + plansys2::solveDerivedPredicates(state); + + ASSERT_TRUE(state.hasPredicate(plansys2::Predicate("(robot_started bluerov)"))); + ASSERT_TRUE( + state.hasPredicate( + plansys2::Predicate("(functiongrounding f_maintain_motion fd_all_thrusters)"))); + ASSERT_TRUE( + state.hasPredicate( + plansys2::Predicate("(functiongrounding f_generate_search_path fd_spiral_high)"))); + + ASSERT_TRUE(state.hasInferredPredicate(plansys2::Predicate("(robot_started bluerov)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-functiongrounding f_maintain_motion fd_all_thrusters)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-functiongrounding f_generate_search_path fd_spiral_high)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_generate_search_path true_boolean)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_maintain_motion true_boolean)"))); + + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_all_thrusters false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_spiral_high false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate(plansys2::Predicate("(inferred-f_active f_follow_pipeline)"))); + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + } + + bt_xml_tree = + R"( + + + + + + + + + + + )"; + + try { + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + status = tree.tickOnce(); + ASSERT_EQ(status, BT::NodeStatus::SUCCESS); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + auto state = problem_client->getState(); + state.addActionsAndPruneDerived( + {(*action_map)["(search_pipeline pipeline bluerov):0"].action_info}); + plansys2::solveDerivedPredicates(state); + + ASSERT_TRUE(state.hasPredicate(plansys2::Predicate("(robot_started bluerov)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_maintain_motion fd_unground)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_maintain_motion fd_all_thrusters)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_generate_search_path fd_unground)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_generate_search_path fd_spiral_high)"))); + + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_generate_search_path true_boolean)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_maintain_motion true_boolean)"))); + + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_all_thrusters false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_spiral_high false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_follow_pipeline true_boolean)"))); + + ASSERT_TRUE(state.hasInferredPredicate(plansys2::Predicate("(pipeline_found pipeline)"))); + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + } + + bt_xml_tree = + R"( + + + + + + + + + )"; + + try { + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + status = tree.tickOnce(); + ASSERT_EQ(status, BT::NodeStatus::SUCCESS); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + auto state = problem_client->getState(); + state.addActionsAndPruneDerived( + {(*action_map)["(inspect_pipeline pipeline bluerov):0"].action_info}); + plansys2::solveDerivedPredicates(state); + + ASSERT_TRUE(state.hasPredicate(plansys2::Predicate("(robot_started bluerov)"))); + ASSERT_TRUE( + state.hasPredicate( + plansys2::Predicate("(functiongrounding f_generate_search_path fd_unground)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_generate_search_path fd_unground)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_maintain_motion fd_unground)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_maintain_motion fd_all_thrusters)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_follow_pipeline fd_follow_pipeline)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_follow_pipeline fd_unground)"))); + + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_follow_pipeline true_boolean)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_maintain_motion true_boolean)"))); + + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_all_thrusters false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_follow_pipeline false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_generate_search_path true_boolean)"))); + + ASSERT_TRUE(state.hasInferredPredicate(plansys2::Predicate("(pipeline_found pipeline)"))); + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + } + + bt_xml_tree = + R"( + + + + + + + + + + + )"; + + try { + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + status = tree.tickOnce(); + ASSERT_EQ(status, BT::NodeStatus::SUCCESS); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + auto state = problem_client->getState(); + state.addActionsAndPruneDerived( + {(*action_map)["(inspect_pipeline pipeline bluerov):0"].action_info}); + plansys2::solveDerivedPredicates(state); + + ASSERT_TRUE(state.hasPredicate(plansys2::Predicate("(robot_started bluerov)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_generate_search_path fd_unground)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_maintain_motion fd_unground)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_maintain_motion fd_all_thrusters)"))); + ASSERT_TRUE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_follow_pipeline fd_follow_pipeline)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(functiongrounding f_follow_pipeline fd_unground)"))); + + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_all_thrusters false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-fd_realisability fd_follow_pipeline false_boolean)"))); + ASSERT_FALSE( + state.hasInferredPredicate( + plansys2::Predicate("(inferred-f_active f_generate_search_path true_boolean)"))); + + ASSERT_TRUE(state.hasInferredPredicate(plansys2::Predicate("(pipeline_found pipeline)"))); + ASSERT_TRUE(state.hasInferredPredicate(plansys2::Predicate("(pipeline_inspected pipeline)"))); + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + } + + finish = true; + t.join(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/plansys2_executor/test/unit/execution_tree_test.cpp b/plansys2_executor/test/unit/execution_tree_test.cpp index a5834eb09..35e19b322 100644 --- a/plansys2_executor/test/unit/execution_tree_test.cpp +++ b/plansys2_executor/test/unit/execution_tree_test.cpp @@ -12,30 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include -#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" -#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/BTBuilder.hpp" - -#include "pluginlib/class_loader.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" #include "pluginlib/class_list_macros.hpp" - +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" - TEST(executiotest_noden_tree, bt_builder_factory) { auto test_node = rclcpp::Node::make_shared("get_action_from_string"); @@ -59,10 +55,11 @@ TEST(executiotest_noden_tree, bt_builder_factory) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -153,16 +150,15 @@ TEST(executiotest_noden_tree, bt_builder_factory) ASSERT_TRUE( problem_client->setGoal( - plansys2::Goal( - "(and (car_assembled car_1) (car_assembled car_2) (car_assembled car_3))"))); + plansys2::Goal("(and (car_assembled car_1) (car_assembled car_2) (car_assembled car_3))"))); auto plan = planner_client->getPlan( domain_client->getDomain(true), problem_client->getProblem(true)); ASSERT_TRUE(plan); std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { @@ -177,7 +173,6 @@ TEST(executiotest_noden_tree, bt_builder_factory) t.join(); } - TEST(executiotest_noden_tree, bt_builder_factory_2) { auto test_node = rclcpp::Node::make_shared("get_action_from_string"); @@ -201,10 +196,11 @@ TEST(executiotest_noden_tree, bt_builder_factory_2) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -304,8 +300,8 @@ TEST(executiotest_noden_tree, bt_builder_factory_2) ASSERT_TRUE(plan); std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { @@ -342,10 +338,11 @@ TEST(executiotest_noden_tree, bt_builder_factory_3) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -375,22 +372,13 @@ TEST(executiotest_noden_tree, bt_builder_factory_3) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wp_control", "waypoint"))); for (unsigned i = 1; i <= 4; i++) { ASSERT_TRUE( - problem_client->addInstance( - plansys2::Instance( - "wp" + std::to_string( - i), "waypoint"))); + problem_client->addInstance(plansys2::Instance("wp" + std::to_string(i), "waypoint"))); } std::vector predicates = { - "(robot_at r2d2 wp_control)", - "(charger_at wp3)", - "(connected wp_control wp1)", - "(connected wp1 wp_control)", - "(connected wp_control wp2)", - "(connected wp2 wp_control)", - "(connected wp_control wp3)", - "(connected wp3 wp_control)", - "(connected wp_control wp4)", + "(robot_at r2d2 wp_control)", "(charger_at wp3)", "(connected wp_control wp1)", + "(connected wp1 wp_control)", "(connected wp_control wp2)", "(connected wp2 wp_control)", + "(connected wp_control wp3)", "(connected wp3 wp_control)", "(connected wp_control wp4)", "(connected wp4 wp_control)"}; for (const auto & pred : predicates) { @@ -428,16 +416,15 @@ TEST(executiotest_noden_tree, bt_builder_factory_3) ASSERT_TRUE( problem_client->setGoal( - plansys2::Goal( - "(and (patrolled wp1) (patrolled wp2) (patrolled wp3) (patrolled wp4))"))); + plansys2::Goal("(and (patrolled wp1) (patrolled wp2) (patrolled wp3) (patrolled wp4))"))); auto plan = planner_client->getPlan( domain_client->getDomain(true), problem_client->getProblem(true)); ASSERT_TRUE(plan); std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { diff --git a/plansys2_executor/test/unit/executor_test.cpp b/plansys2_executor/test/unit/executor_test.cpp index f1e465fbe..5345b0eb7 100644 --- a/plansys2_executor/test/unit/executor_test.cpp +++ b/plansys2_executor/test/unit/executor_test.cpp @@ -12,57 +12,58 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" -#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/blackboard.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" + +#include "gtest/gtest.h" + +#include "lifecycle_msgs/msg/state.hpp" + #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/BTBuilder.hpp" #include "plansys2_executor/ExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - -#include "behaviortree_cpp/behavior_tree.h" -#include "behaviortree_cpp/bt_factory.h" -#include "behaviortree_cpp/utils/shared_library.h" -#include "behaviortree_cpp/blackboard.h" - +#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" #include "plansys2_executor/behavior_tree/restore_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" -#include "pluginlib/class_loader.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" + +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" + #include "pluginlib/class_list_macros.hpp" +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - - TEST(executor, action_executor_api) { auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); @@ -76,8 +77,7 @@ TEST(executor, action_executor_api) ASSERT_EQ(action_executor_1->get_status(), BT::NodeStatus::IDLE); } -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using namespace std::chrono_literals; class MoveAction : public plansys2::ActionExecutorClient @@ -103,20 +103,13 @@ class MoveAction : public plansys2::ActionExecutorClient } MoveAction(const std::string & id, const std::chrono::nanoseconds & rate) - : ActionExecutorClient(id, rate), - executions_(0), - cycles_(0), - runtime_(0) + : ActionExecutorClient(id, rate), executions_(0), cycles_(0), runtime_(0) { } - void set_runtime(double runtime) - { - runtime_ = runtime; - } + void set_runtime(double runtime) {runtime_ = runtime;} - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "MoveAction::on_activate" << std::endl; counter_ = 0; @@ -199,8 +192,7 @@ class TransportAction : public plansys2::ActionExecutorClient cycles_ = 0; } - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "TransportAction::on_activate" << std::endl; counter_ = 0; @@ -384,17 +376,16 @@ TEST(executor, action_executor_client_old_constructor) std::vector history_msgs; bool confirmed = false; auto actions_sub = aux_node->create_subscription( - "actions_hub", - rclcpp::QoS(100).reliable(), [&](plansys2_msgs::msg::ActionExecution::UniquePtr msg) { - history_msgs.push_back(*msg); - }); + "actions_hub", rclcpp::QoS(100).reliable(), + [&](plansys2_msgs::msg::ActionExecution::UniquePtr msg) {history_msgs.push_back(*msg);}); bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); - std::string bt_xml_tree = R"( @@ -427,7 +418,6 @@ TEST(executor, action_executor_client_old_constructor) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); - auto status = BT::NodeStatus::RUNNING; while (status != BT::NodeStatus::SUCCESS) { status = tree.tickOnce(); @@ -442,7 +432,6 @@ TEST(executor, action_executor_client_old_constructor) ASSERT_EQ(transport_action_node->cycles_, 15); // ASSERT_EQ(history_msgs.size(), 64); - finish = true; t.join(); } @@ -470,10 +459,11 @@ TEST(executor, action_executor) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -568,10 +558,9 @@ TEST(executor, action_executor) domain_client->getDomain(true), problem_client->getProblem(true)); ASSERT_TRUE(plan); - std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { @@ -588,10 +577,10 @@ TEST(executor, action_executor) class ExecuteActionTest : public plansys2::ExecuteAction { public: - ExecuteActionTest( - const std::string & xml_tag_name, - const BT::NodeConfig & conf) - : ExecuteAction(xml_tag_name, conf) {} + ExecuteActionTest(const std::string & xml_tag_name, const BT::NodeConfig & conf) + : ExecuteAction(xml_tag_name, conf) + { + } void halt() override { @@ -613,10 +602,10 @@ class ExecuteActionTest : public plansys2::ExecuteAction class WaitActionTest : public plansys2::WaitAction { public: - WaitActionTest( - const std::string & xml_tag_name, - const BT::NodeConfig & conf) - : WaitAction(xml_tag_name, conf) {} + WaitActionTest(const std::string & xml_tag_name, const BT::NodeConfig & conf) + : WaitAction(xml_tag_name, conf) + { + } void halt() override { @@ -637,10 +626,10 @@ class WaitActionTest : public plansys2::WaitAction class CheckOverAllReqTest : public plansys2::CheckOverAllReq { public: - CheckOverAllReqTest( - const std::string & xml_tag_name, - const BT::NodeConfig & conf) - : CheckOverAllReq(xml_tag_name, conf) {} + CheckOverAllReqTest(const std::string & xml_tag_name, const BT::NodeConfig & conf) + : CheckOverAllReq(xml_tag_name, conf) + { + } void halt() override { @@ -661,10 +650,10 @@ class CheckOverAllReqTest : public plansys2::CheckOverAllReq class WaitAtStartReqTest : public plansys2::WaitAtStartReq { public: - WaitAtStartReqTest( - const std::string & xml_tag_name, - const BT::NodeConfig & conf) - : WaitAtStartReq(xml_tag_name, conf) {} + WaitAtStartReqTest(const std::string & xml_tag_name, const BT::NodeConfig & conf) + : WaitAtStartReq(xml_tag_name, conf) + { + } void halt() override { @@ -685,10 +674,10 @@ class WaitAtStartReqTest : public plansys2::WaitAtStartReq class CheckAtEndReqTest : public plansys2::CheckAtEndReq { public: - CheckAtEndReqTest( - const std::string & xml_tag_name, - const BT::NodeConfig & conf) - : CheckAtEndReq(xml_tag_name, conf) {} + CheckAtEndReqTest(const std::string & xml_tag_name, const BT::NodeConfig & conf) + : CheckAtEndReq(xml_tag_name, conf) + { + } void halt() override { @@ -709,10 +698,10 @@ class CheckAtEndReqTest : public plansys2::CheckAtEndReq class ApplyAtStartEffectTest : public plansys2::ApplyAtStartEffect { public: - ApplyAtStartEffectTest( - const std::string & xml_tag_name, - const BT::NodeConfig & conf) - : ApplyAtStartEffect(xml_tag_name, conf) {} + ApplyAtStartEffectTest(const std::string & xml_tag_name, const BT::NodeConfig & conf) + : ApplyAtStartEffect(xml_tag_name, conf) + { + } void halt() override { @@ -757,10 +746,10 @@ class RestoreAtStartEffectTest : public plansys2::RestoreAtStartEffect class ApplyAtEndEffectTest : public plansys2::ApplyAtEndEffect { public: - ApplyAtEndEffectTest( - const std::string & xml_tag_name, - const BT::NodeConfig & conf) - : ApplyAtEndEffect(xml_tag_name, conf) {} + ApplyAtEndEffectTest(const std::string & xml_tag_name, const BT::NodeConfig & conf) + : ApplyAtEndEffect(xml_tag_name, conf) + { + } void halt() override { @@ -825,13 +814,13 @@ TEST(executor, action_real_action_1) 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();} + 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); @@ -946,7 +935,6 @@ TEST(executor, action_real_action_1) )"; - std::vector predicates = { "(robot_at r2d2 steering_wheels_zone)", "(robot_available r2d2)", @@ -968,9 +956,7 @@ TEST(executor, action_real_action_1) ASSERT_EQ(ApplyAtStartEffectTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at r2d2 steering_wheels_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 steering_wheels_zone)"))); status = tree.tickOnce(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); @@ -1010,9 +996,7 @@ TEST(executor, action_real_action_1) } ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at r2d2 assembly_zone)"))); + 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) { @@ -1031,7 +1015,6 @@ TEST(executor, action_real_action_1) std::cerr << e.what() << '\n'; } - ExecuteActionTest::test_status = BT::NodeStatus::IDLE; WaitActionTest::test_status = BT::NodeStatus::IDLE; CheckOverAllReqTest::test_status = BT::NodeStatus::IDLE; @@ -1081,13 +1064,13 @@ TEST(executor, action_real_action_1_with_restore) 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();} + 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); @@ -1211,7 +1194,6 @@ TEST(executor, action_real_action_1_with_restore) )"; - std::vector predicates = { "(robot_at r2d2 steering_wheels_zone)", "(robot_available r2d2)", @@ -1282,9 +1264,7 @@ TEST(executor, action_real_action_1_with_restore) } ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at r2d2 assembly_zone)"))); + 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) { @@ -1303,7 +1283,6 @@ TEST(executor, action_real_action_1_with_restore) std::cerr << e.what() << '\n'; } - ExecuteActionTest::test_status = BT::NodeStatus::IDLE; WaitActionTest::test_status = BT::NodeStatus::IDLE; CheckOverAllReqTest::test_status = BT::NodeStatus::IDLE; @@ -1356,13 +1335,13 @@ TEST(executor, action_real_action_2) 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();} + 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); @@ -2268,7 +2247,9 @@ TEST(executor, executor_client_execute_plan_two_plans) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -2438,7 +2419,9 @@ TEST(executor, executor_client_execute_plan_replan) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -2750,8 +2733,7 @@ class PatrolAction : public plansys2::ActionExecutorClient cycles_ = 0; } - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "PatrolAction::on_activate" << std::endl; counter_ = 0; @@ -2781,7 +2763,6 @@ class PatrolAction : public plansys2::ActionExecutorClient int cycles_; }; - TEST(executor, executor_client_ordered_sub_goals) { auto test_node_1 = rclcpp::Node::make_shared("test_node_1"); @@ -2823,7 +2804,9 @@ TEST(executor, executor_client_ordered_sub_goals) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -2862,28 +2845,17 @@ TEST(executor, executor_client_ordered_sub_goals) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wp2", "waypoint"))); std::vector predicates = { - "(robot_at r2d2 wp0)", - "(connected wp0 wp1)", - "(connected wp1 wp0)", - "(connected wp0 wp2)", - "(connected wp2 wp0)", - "(connected wp1 wp2)", - "(connected wp2 wp1)", + "(robot_at r2d2 wp0)", "(connected wp0 wp1)", "(connected wp1 wp0)", "(connected wp0 wp2)", + "(connected wp2 wp0)", "(connected wp1 wp2)", "(connected wp2 wp1)", }; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } std::vector functions = { - "(= (speed r2d2) 1.0)", - "(= (max_range r2d2) 100.0)", - "(= (state_of_charge r2d2) 100.0)", - "(= (distance wp0 wp1) 5.0)", - "(= (distance wp1 wp0) 5.0)", - "(= (distance wp0 wp2) 15.0)", - "(= (distance wp2 wp0) 15.0)", - "(= (distance wp1 wp2) 5.0)", - "(= (distance wp2 wp1) 5.0)", + "(= (speed r2d2) 1.0)", "(= (max_range r2d2) 100.0)", "(= (state_of_charge r2d2) 100.0)", + "(= (distance wp0 wp1) 5.0)", "(= (distance wp1 wp0) 5.0)", "(= (distance wp0 wp2) 15.0)", + "(= (distance wp2 wp0) 15.0)", "(= (distance wp1 wp2) 5.0)", "(= (distance wp2 wp1) 5.0)", }; for (const auto & func : functions) { ASSERT_TRUE(problem_client->addFunction(plansys2::Function(func))); @@ -2970,13 +2942,13 @@ TEST(executor, executor_client_cancel_plan) 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();} + 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); @@ -3039,7 +3011,8 @@ TEST(executor, executor_client_cancel_plan) while (rclcpp::ok() && executor_client->execute_and_check_plan()) { auto feedback = executor_client->getFeedBack(); - if (!feedback.action_execution_status.empty() && + if ( + !feedback.action_execution_status.empty() && feedback.action_execution_status[0].status == plansys2_msgs::msg::ActionExecutionInfo::EXECUTING && (test_node_1->now() - start).seconds() > 3) @@ -3062,8 +3035,7 @@ TEST(executor, executor_client_cancel_plan) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); finish = true; t.join(); @@ -3101,8 +3073,7 @@ TEST(executor, action_timeout) executor_node->set_parameter({"bt_builder_plugin", "SimpleBTBuilder"}); executor_node->set_parameter({"action_timeouts.actions", std::vector({"move"})}); // have to declare because the actions vector above was not available at node creation - executor_node->declare_parameter( - "action_timeouts.move.duration_overrun_percentage", 1.0); + executor_node->declare_parameter("action_timeouts.move.duration_overrun_percentage", 1.0); executor_node->set_parameter({"action_timeouts.move.duration_overrun_percentage", 1.0}); rclcpp::experimental::executors::EventsExecutor exe; @@ -3116,7 +3087,9 @@ TEST(executor, action_timeout) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -3184,9 +3157,8 @@ TEST(executor, action_timeout) std::stringstream ss; ss.setf(std::ios_base::fixed, std::ios_base::floatfield); for (const auto & action_feedback : feedback.action_execution_status) { - ss << "[" << action_feedback.action << " " << std::setprecision(1) << - action_feedback.completion * 100.0 << - "%]"; + ss << "[" << action_feedback.action << " " << std::setprecision(1) + << action_feedback.completion * 100.0 << "%]"; } auto & clk = *executor_node->get_clock(); RCLCPP_WARN_THROTTLE(executor_node->get_logger(), clk, 500, "%s", ss.str().c_str()); @@ -3198,8 +3170,7 @@ TEST(executor, action_timeout) auto result = executor_client->getResult().value(); ASSERT_EQ(result.result, plansys2_msgs::action::ExecutePlan::Result::FAILURE); ASSERT_EQ( - result.action_execution_status[0].status, - plansys2_msgs::msg::ActionExecutionInfo::CANCELLED); + result.action_execution_status[0].status, plansys2_msgs::msg::ActionExecutionInfo::CANCELLED); { rclcpp::Rate rate(10); @@ -3210,14 +3181,12 @@ TEST(executor, action_timeout) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); finish = true; t.join(); } - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp index 409923490..7cc8c9575 100644 --- a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp +++ b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp @@ -12,49 +12,43 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include +#include #include +#include +#include #include +#include +#include #include -#include +#include #include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - -#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/blackboard.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_executor/ExecutorNode.hpp" #include "plansys2_executor/ExecutorClient.hpp" - -#include "behaviortree_cpp/behavior_tree.h" -#include "behaviortree_cpp/bt_factory.h" -#include "behaviortree_cpp/utils/shared_library.h" -#include "behaviortree_cpp/blackboard.h" - +#include "plansys2_executor/ExecutorNode.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" - -#include "lifecycle_msgs/msg/state.hpp" - +#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - class SimpleBTBuilderTest : public plansys2::SimpleBTBuilder { public: @@ -63,34 +57,31 @@ class SimpleBTBuilderTest : public plansys2::SimpleBTBuilder std::string get_tree(const plansys2_msgs::msg::Plan & current_plan) { - return SimpleBTBuilder::get_tree(current_plan); + return plansys2::SimpleBTBuilder::get_tree(current_plan); } - std::vector get_plan_actions(const plansys2_msgs::msg::Plan & plan) + std::vector get_plan_actions( + const plansys2_msgs::msg::Plan & plan) { - return SimpleBTBuilder::get_plan_actions(plan); + return plansys2::SimpleBTBuilder::get_plan_actions(plan); } bool is_action_executable( - const plansys2::ActionStamped & action, - std::vector & predicates, - std::vector & functions) const + const plansys2::ActionStamped & action, const plansys2::State & state) { - return SimpleBTBuilder::is_action_executable(action, predicates, functions); + return plansys2::SimpleBTBuilder::is_action_executable(action, state); } plansys2::ActionGraph::Ptr get_graph(const plansys2_msgs::msg::Plan & current_plan) { - return SimpleBTBuilder::get_graph(current_plan); + return plansys2::SimpleBTBuilder::get_graph(current_plan); } std::list get_roots( std::vector & action_sequence, - std::vector & predicates, - std::vector & functions, - int & node_counter) + const plansys2::State & state, int & node_counter) { - return SimpleBTBuilder::get_roots(action_sequence, predicates, functions, node_counter); + return plansys2::SimpleBTBuilder::get_roots(action_sequence, state, node_counter); } plansys2::ActionNode::Ptr get_node_satisfy( @@ -98,7 +89,7 @@ class SimpleBTBuilderTest : public plansys2::SimpleBTBuilder const plansys2::ActionGraph::Ptr & graph, const plansys2::ActionNode::Ptr & current) { - return SimpleBTBuilder::get_node_satisfy(requirement, graph, current); + return plansys2::SimpleBTBuilder::get_node_satisfy(requirement, graph, current); } plansys2::ActionNode::Ptr get_node_satisfy( @@ -106,32 +97,52 @@ class SimpleBTBuilderTest : public plansys2::SimpleBTBuilder const plansys2::ActionNode::Ptr & node, const plansys2::ActionNode::Ptr & current) { - return SimpleBTBuilder::get_node_satisfy(requirement, node, current); + return plansys2::SimpleBTBuilder::get_node_satisfy(requirement, node, current); } - void print_graph(const plansys2::ActionGraph::Ptr & graph) const { - SimpleBTBuilder::print_graph(graph); + plansys2::SimpleBTBuilder::print_graph(graph); } void print_graph_csv(const plansys2::ActionGraph::Ptr & graph) const { - SimpleBTBuilder::print_graph_csv(graph); + plansys2::SimpleBTBuilder::print_graph_csv(graph); } std::vector> get_graph_tabular( const plansys2::ActionGraph::Ptr & graph) const { - return SimpleBTBuilder::get_graph_tabular(graph); + return plansys2::SimpleBTBuilder::get_graph_tabular(graph); } void remove_existing_requirements( - std::vector & requirements, - std::vector & predicates, - std::vector & functions) const + std::vector & requirements, const plansys2::State & state) { - SimpleBTBuilder::remove_existing_requirements(requirements, predicates, functions); + plansys2::SimpleBTBuilder::remove_existing_requirements(requirements, state); + } +}; + +class TestPlannerNode : public rclcpp::Node +{ +private: + rclcpp::Service::SharedPtr validate_domain_service_ = + create_service( + "planner/validate_domain", + std::bind( + &TestPlannerNode::validate_domain_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + +public: + TestPlannerNode() + : Node("test_planner_node") {} + + void validate_domain_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + response->success = true; } }; @@ -161,10 +172,11 @@ TEST(simple_btbuilder_tests, test_plan_1) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -199,18 +211,12 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("chargingroom", "room"))); std::vector predicate_strings = { - "(connected entrance dinning)", - "(connected dinning entrance)", - "(connected dinning kitchen)", - "(connected kitchen dinning)", - "(connected dinning bedroom)", - "(connected bedroom dinning)", - "(connected bathroom bedroom)", - "(connected bedroom bathroom)", - "(connected chargingroom kitchen)", - "(connected kitchen chargingroom)", - "(charging_point_at chargingroom)", - "(battery_low leia)", + "(connected entrance dinning)", "(connected dinning entrance)", + "(connected dinning kitchen)", "(connected kitchen dinning)", + "(connected dinning bedroom)", "(connected bedroom dinning)", + "(connected bathroom bedroom)", "(connected bedroom bathroom)", + "(connected chargingroom kitchen)", "(connected kitchen chargingroom)", + "(charging_point_at chargingroom)", "(battery_low leia)", "(robot_at leia entrance)"}; for (const auto & pred : predicate_strings) { @@ -223,9 +229,7 @@ TEST(simple_btbuilder_tests, test_plan_1) domain_client->getDomain(true), problem_client->getProblem(true)); ASSERT_TRUE(plan); - - auto predicates = problem_client->getPredicates(); - auto functions = problem_client->getFunctions(); + auto state = problem_client->getState(); auto action_sequence = btbuilder->get_plan_actions(plan.value()); @@ -249,136 +253,73 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_TRUE(action_0_predicates[0].negate); ASSERT_TRUE( - plansys2::check( - action_sequence[0].action.get_at_start_requirements(), - problem_client)); + plansys2::check(action_sequence[0].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( - plansys2::check( - action_sequence[1].action.get_at_start_requirements(), - problem_client)); + plansys2::check(action_sequence[1].action.get_at_start_requirements(), problem_client)); + ASSERT_FALSE( + plansys2::check(action_sequence[2].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( - plansys2::check( - action_sequence[2].action.get_at_start_requirements(), - problem_client)); + plansys2::check(action_sequence[3].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( - plansys2::check( - action_sequence[3].action.get_at_start_requirements(), - problem_client)); + plansys2::check(action_sequence[4].action.get_at_start_requirements(), problem_client)); ASSERT_FALSE( - plansys2::check( - action_sequence[4].action.get_at_start_requirements(), - problem_client)); + plansys2::check(action_sequence[5].action.get_at_start_requirements(), problem_client)); + + ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[0], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[1], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[2], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[3], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], state)); + + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(robot_at leia entrance)"))); ASSERT_FALSE( - plansys2::check( - action_sequence[5].action.get_at_start_requirements(), - problem_client)); - - ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[0], predicates, functions)); - ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[1], predicates, functions)); - ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[2], predicates, functions)); - ASSERT_FALSE(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)); - - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia entrance)"), true)) != predicates.end()); - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"), true)) == - predicates.end()); - - plansys2::apply( - action_sequence[0].action.get_at_start_effects(), - predicates, functions); - plansys2::apply( - action_sequence[0].action.get_at_end_effects(), - predicates, functions); - - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia entrance)"), true)) == - predicates.end()); - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"), true)) != - predicates.end()); - - ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[1], predicates, functions)); - ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[2], predicates, functions)); - ASSERT_FALSE(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[1].action.get_at_start_effects(), - predicates, functions); - plansys2::apply( - action_sequence[1].action.get_at_end_effects(), - predicates, functions); - - ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[2], predicates, functions)); - ASSERT_FALSE(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[2].action.get_at_start_effects(), - predicates, functions); - plansys2::apply( - 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.get_at_start_effects(), - predicates, functions); - plansys2::apply( - 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.get_at_start_effects(), - predicates, functions); - plansys2::apply( - 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.get_at_start_effects(), - predicates, functions); - plansys2::apply( - action_sequence[5].action.get_at_end_effects(), - predicates, functions); - - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia bathroom)"), true)) != predicates.end()); + state.hasPredicate(parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"))); + + plansys2::apply(action_sequence[0].action.get_at_start_effects(), state); + plansys2::apply(action_sequence[0].action.get_at_end_effects(), state); + + ASSERT_FALSE(state.hasPredicate(parser::pddl::fromStringPredicate("(robot_at leia entrance)"))); + ASSERT_TRUE( + state.hasPredicate(parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"))); + + ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[1], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[2], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[3], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], state)); + plansys2::apply(action_sequence[1].action.get_at_start_effects(), state); + plansys2::apply(action_sequence[1].action.get_at_end_effects(), state); + + ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[2], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[3], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], state)); + plansys2::apply(action_sequence[2].action.get_at_start_effects(), state); + plansys2::apply(action_sequence[2].action.get_at_end_effects(), state); + + ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[3], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], state)); + plansys2::apply(action_sequence[3].action.get_at_start_effects(), state); + plansys2::apply(action_sequence[3].action.get_at_end_effects(), state); + + ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[4], state)); + ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], state)); + plansys2::apply(action_sequence[4].action.get_at_start_effects(), state); + plansys2::apply(action_sequence[4].action.get_at_end_effects(), state); + + ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[5], state)); + plansys2::apply(action_sequence[5].action.get_at_start_effects(), state); + plansys2::apply(action_sequence[5].action.get_at_end_effects(), state); + + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(robot_at leia bathroom)"))); finish = true; t.join(); } - TEST(simple_btbuilder_tests, test_plan_2) { auto test_node = rclcpp::Node::make_shared("test_plan_2"); @@ -405,10 +346,11 @@ TEST(simple_btbuilder_tests, test_plan_2) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -494,19 +436,16 @@ TEST(simple_btbuilder_tests, test_plan_2) ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } - ASSERT_TRUE( problem_client->setGoal( - plansys2::Goal( - "(and(car_assembled car_1)(car_assembled car_2)(car_assembled car_3))"))); + plansys2::Goal("(and(car_assembled car_1)(car_assembled car_2)(car_assembled car_3))"))); auto plan = planner_client->getPlan( domain_client->getDomain(true), problem_client->getProblem(true)); ASSERT_TRUE(plan); - auto predicates = problem_client->getPredicates(); - auto functions = problem_client->getFunctions(); + auto state = problem_client->getState(); auto predicates_plus_one = predicate_strings; predicates_plus_one.push_back("(is_assembly_zone body_car_zone)"); @@ -514,63 +453,42 @@ TEST(simple_btbuilder_tests, test_plan_2) std::vector check_predicates = parser::pddl::getSubtrees(tree); - btbuilder->remove_existing_requirements(check_predicates, predicates, functions); + btbuilder->remove_existing_requirements(check_predicates, state); ASSERT_EQ(check_predicates.size(), 1); - ASSERT_EQ( - parser::pddl::toString(check_predicates.front()), "(is_assembly_zone body_car_zone)"); + ASSERT_EQ(parser::pddl::toString(check_predicates.front()), "(is_assembly_zone body_car_zone)"); - - ASSERT_EQ(problem_client->getPredicates().size(), predicates.size()); + ASSERT_EQ(problem_client->getPredicates().size(), state.getPredicatesSize()); auto action_sequence = btbuilder->get_plan_actions(plan.value()); ASSERT_EQ(action_sequence.size(), 22u); int node_counter = 0; - auto roots = btbuilder->get_roots(action_sequence, predicates, functions, node_counter); + auto roots = btbuilder->get_roots(action_sequence, state, node_counter); ASSERT_EQ(roots.size(), 3u); // Apply roots actions for (auto & action_node : roots) { - plansys2::apply( - action_node->action.action.get_at_start_effects(), - predicates, functions); - plansys2::apply( - action_node->action.action.get_at_end_effects(), - predicates, functions); + plansys2::apply(action_node->action.action.get_at_start_effects(), state); + plansys2::apply(action_node->action.action.get_at_end_effects(), state); } - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at robot1 body_car_zone)"), true)) != - predicates.end()); - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at robot2 steering_wheels_zone)"), true)) != - predicates.end()); - EXPECT_TRUE( - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at robot3 wheels_zone)"), true)) != - predicates.end()); + ASSERT_TRUE( + state.hasPredicate( + parser::pddl::fromStringPredicate("(robot_at robot1 steering_wheels_zone)"))); + ASSERT_TRUE( + state.hasPredicate(parser::pddl::fromStringPredicate("(robot_at robot2 wheels_zone)"))); + ASSERT_TRUE( + state.hasPredicate(parser::pddl::fromStringPredicate("(robot_at robot3 body_car_zone)"))); tree.nodes.clear(); parser::pddl::fromString( - tree, "(robot_at robot1 body_car_zone)", false, - plansys2_msgs::msg::Node::AND); + tree, "(robot_at robot3 body_car_zone)", false, plansys2_msgs::msg::Node::AND); auto node_satisfy_1 = btbuilder->get_node_satisfy(tree, *roots.begin(), nullptr); EXPECT_TRUE(node_satisfy_1 != nullptr); 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()[0].name, "robot3"); 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"); @@ -615,10 +533,11 @@ TEST(simple_btbuilder_tests, test_plan_3) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -650,14 +569,8 @@ TEST(simple_btbuilder_tests, test_plan_3) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("ro3", "room"))); std::vector predicates = { - "(connected ro1 ro2)", - "(connected ro2 ro1)", - "(connected ro1 ro3)", - "(connected ro3 ro1)", - "(connected ro2 ro3)", - "(connected ro3 ro2)", - "(robot_at leia ro1)", - "(battery_full leia)"}; + "(connected ro1 ro2)", "(connected ro2 ro1)", "(connected ro1 ro3)", "(connected ro3 ro1)", + "(connected ro2 ro3)", "(connected ro3 ro2)", "(robot_at leia ro1)", "(battery_full leia)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); @@ -665,8 +578,7 @@ TEST(simple_btbuilder_tests, test_plan_3) ASSERT_TRUE( problem_client->setGoal( - plansys2::Goal( - "(and (patrolled ro1) (patrolled ro2) (patrolled ro3))"))); + plansys2::Goal("(and (patrolled ro1) (patrolled ro2) (patrolled ro3))"))); auto plan = planner_client->getPlan( domain_client->getDomain(true), problem_client->getProblem(true)); @@ -707,10 +619,11 @@ TEST(simple_btbuilder_tests, test_plan_4) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + 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); @@ -772,17 +685,14 @@ TEST(simple_btbuilder_tests, test_plan_4) "(robot_at r2d2 cooking_zone)", "(battery_full r2d2)", "(robot_at c3po cooking_zone)", - "(battery_full c3po)" - }; + "(battery_full c3po)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } ASSERT_TRUE( - problem_client->setGoal( - plansys2::Goal( - "(and (dish_prepared cake)(dish_prepared omelette))"))); + problem_client->setGoal(plansys2::Goal("(and (dish_prepared cake)(dish_prepared omelette))"))); auto plan = planner_client->getPlan( domain_client->getDomain(true), problem_client->getProblem(true)); @@ -826,7 +736,9 @@ TEST(simple_btbuilder_tests, test_plan_5) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -855,9 +767,8 @@ TEST(simple_btbuilder_tests, test_plan_5) } std::ifstream problem_ifs(pkgpath + "/pddl/road_trip_problem.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_client->addProblem(problem_str)); auto plan = planner_client->getPlan( @@ -940,7 +851,9 @@ TEST(simple_btbuilder_tests, test_plan_6) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -969,9 +882,8 @@ TEST(simple_btbuilder_tests, test_plan_6) } std::ifstream problem_ifs(pkgpath + "/pddl/elevator_problem.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_client->addProblem(problem_str)); auto plan = planner_client->getPlan( @@ -1028,7 +940,6 @@ TEST(simple_btbuilder_tests, test_plan_6) t.join(); } - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/plansys2_msgs/CMakeLists.txt b/plansys2_msgs/CMakeLists.txt index 794e69289..19e3b75f7 100644 --- a/plansys2_msgs/CMakeLists.txt +++ b/plansys2_msgs/CMakeLists.txt @@ -27,7 +27,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/AddProblem.srv" "srv/AddProblemGoal.srv" "srv/AffectNode.srv" + "srv/AffectNodes.srv" "srv/AffectParam.srv" + "srv/ClearProblemKnowledge.srv" "srv/ExistNode.srv" "srv/GetDomain.srv" "srv/GetDomainActions.srv" @@ -45,10 +47,11 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/GetProblemGoal.srv" "srv/GetProblemInstances.srv" "srv/GetProblemInstanceDetails.srv" + "srv/GetProblemState.srv" "srv/GetStates.srv" "srv/IsProblemGoalSatisfied.srv" "srv/RemoveProblemGoal.srv" - "srv/ClearProblemKnowledge.srv" + "srv/UpdateNodes.srv" "srv/ValidateDomain.srv" "action/ExecutePlan.action" DEPENDENCIES builtin_interfaces std_msgs action_msgs diff --git a/plansys2_msgs/srv/AffectNodes.srv b/plansys2_msgs/srv/AffectNodes.srv new file mode 100644 index 000000000..50776e043 --- /dev/null +++ b/plansys2_msgs/srv/AffectNodes.srv @@ -0,0 +1,4 @@ +plansys2_msgs/Node[] nodes +--- +bool success +string error_info diff --git a/plansys2_msgs/srv/GetProblemState.srv b/plansys2_msgs/srv/GetProblemState.srv new file mode 100644 index 000000000..962eff9f5 --- /dev/null +++ b/plansys2_msgs/srv/GetProblemState.srv @@ -0,0 +1,5 @@ +std_msgs/Empty request +--- +bool success +plansys2_msgs/State state +string error_info diff --git a/plansys2_msgs/srv/UpdateNodes.srv b/plansys2_msgs/srv/UpdateNodes.srv new file mode 100644 index 000000000..f3d91e653 --- /dev/null +++ b/plansys2_msgs/srv/UpdateNodes.srv @@ -0,0 +1,5 @@ +plansys2_msgs/Node[] add_nodes +plansys2_msgs/Node[] remove_nodes +--- +bool success +string error_info diff --git a/plansys2_planner/test/unit/planner_test.cpp b/plansys2_planner/test/unit/planner_test.cpp index c64e75126..26a811444 100644 --- a/plansys2_planner/test/unit/planner_test.cpp +++ b/plansys2_planner/test/unit/planner_test.cpp @@ -527,7 +527,23 @@ TEST(planner_expert, generate_plan_with_domain_constants) std::istreambuf_iterator()); ASSERT_TRUE(problem_client->addProblem(problem_1_str)); - ASSERT_EQ(problem_client->getProblem(), problem_1_str); + const std::string expected_str = + "( define ( problem problem_1 )\n" + "( :domain plansys2 )\n" + "( :objects\n" + "\tm1 - message\n" + "\tbedroom kitchen - room\n" + ")\n" + "( :init\n" + "\t( person_at jack bedroom )\n" + "\t( robot_at leia kitchen )\n" + ")\n" + "( :goal\n" + "\t( and\n" + "\t\t( robot_talk leia m1 jack )\n" + "\t))\n" + ")\n"; + ASSERT_EQ(problem_client->getProblem(), expected_str); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); diff --git a/plansys2_problem_expert/CMakeLists.txt b/plansys2_problem_expert/CMakeLists.txt index 0cd873453..036297ac6 100644 --- a/plansys2_problem_expert/CMakeLists.txt +++ b/plansys2_problem_expert/CMakeLists.txt @@ -12,6 +12,12 @@ find_package(plansys2_domain_expert REQUIRED) find_package(std_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) +find_package(OpenMP REQUIRED) + +set(CMAKE_CXX_STANDARD 17) + +include_directories(include) + set(PROBLEM_EXPERT_SOURCES src/plansys2_problem_expert/ProblemExpert.cpp src/plansys2_problem_expert/ProblemExpertClient.cpp @@ -33,6 +39,7 @@ target_link_libraries(${PROJECT_NAME} plansys2_pddl_parser::plansys2_pddl_parser rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle + OpenMP::OpenMP_CXX ${std_msgs_TARGETS} ) @@ -90,6 +97,7 @@ ament_export_dependencies( plansys2_domain_expert std_msgs lifecycle_msgs + OpenMP ) ament_package() diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp index 7697becfd..b7b9b82fa 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp @@ -15,14 +15,19 @@ #ifndef PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERT_HPP_ #define PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERT_HPP_ +#include +#include #include #include +#include #include -#include +#include "plansys2_domain_expert/DomainExpert.hpp" +#include "plansys2_msgs/msg/node.hpp" +#include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" +#include "plansys2_pddl_parser/Utils.hpp" #include "plansys2_problem_expert/ProblemExpertInterface.hpp" -#include "plansys2_domain_expert/DomainExpert.hpp" namespace plansys2 { @@ -48,9 +53,9 @@ class ProblemExpert : public ProblemExpertInterface /** * @brief Get all instances in the problem. * - * @return std::vector Vector containing all instances in the problem. + * @return std::unordered_set Set containing all instances in the problem. */ - std::vector getInstances(); + std::unordered_set getInstances(); /** * @brief Add a new instance to the problem. @@ -79,9 +84,16 @@ class ProblemExpert : public ProblemExpertInterface /** * @brief Get all predicates in the problem. * - * @return std::vector Vector containing all predicates in the problem. + * @return std::unordered_set Set containing all predicates in the problem. + */ + std::unordered_set getPredicates(); + + /** + * @brief Get all inferred predicates in the problem. + * + * @return std::unordered_set Set containing all inferred predicates in the problem. */ - std::vector getPredicates(); + std::unordered_set getInferredPredicates(); /** * @brief Add a new predicate to the problem. @@ -91,6 +103,14 @@ class ProblemExpert : public ProblemExpertInterface */ bool addPredicate(const plansys2::Predicate & predicate); + /** + * @brief Add new predicates to the problem. + * + * @param[in] predicates The predicates to be added. + * @return true if the predicates were successfully added, false otherwise. + */ + bool addPredicates(const std::vector & predicates); + /** * @brief Remove a predicate from the problem. * @@ -99,6 +119,25 @@ class ProblemExpert : public ProblemExpertInterface */ bool removePredicate(const plansys2::Predicate & predicate); + /** + * @brief Remove multiple predicates from the problem. + * + * @param[in] predicates The predicates to be removed. + * @return true if the predicates were successfully removed, false otherwise. + */ + bool removePredicates(const std::vector & predicates); + + /** + * @brief Update the predicates in the problem. + * + * @param[in] add_predicates The predicates to be added. + * @param[in] remove_predicates The predicates to be removed. + * @return true if the predicates were successfully updated, false otherwise. + */ + bool updatePredicates( + const std::vector & add_predicates, + const std::vector & remove_predicates); + /** * @brief Check if a predicate exists in the problem. * @@ -107,6 +146,14 @@ class ProblemExpert : public ProblemExpertInterface */ bool existPredicate(const plansys2::Predicate & predicate); + /** + * @brief Check if an inferred predicate exists in the problem. + * + * @param[in] predicate The inferred predicate to check. + * @return true if the inferred predicate exists, false otherwise. + */ + bool existInferredPredicate(const plansys2::Predicate & predicate); + /** * @brief Get a specific predicate by its expression. * @@ -118,9 +165,9 @@ class ProblemExpert : public ProblemExpertInterface /** * @brief Get all functions in the problem. * - * @return std::vector Vector containing all functions in the problem. + * @return std::unordered_set Set containing all functions in the problem. */ - std::vector getFunctions(); + std::unordered_set getFunctions(); /** * @brief Add a new function to the problem. @@ -162,6 +209,13 @@ class ProblemExpert : public ProblemExpertInterface */ std::optional getFunction(const std::string & expr); + /** + * @brief Get the current state of the problem. + * + * @return plansys2::State The current state of the problem. + */ + plansys2::State getState(); + /** * @brief Get the current goal of the problem. * @@ -254,6 +308,8 @@ class ProblemExpert : public ProblemExpertInterface */ bool isValidGoal(const plansys2::Goal & goal); + void updateInferredPredicates(); + private: /** * @brief Check if all predicates and functions in a tree are valid. @@ -264,10 +320,12 @@ class ProblemExpert : public ProblemExpertInterface * @return true if all predicates and functions in the tree are valid, false otherwise. */ bool checkPredicateTreeTypes( - const plansys2_msgs::msg::Tree & tree, - std::shared_ptr & domain_expert_, + const plansys2_msgs::msg::Tree & tree, std::shared_ptr & domain_expert_, uint8_t node_id = 0); + void removeInvalidPredicates(const plansys2::Instance & instance); + void removeInvalidFunctions(const plansys2::Instance & instance); + /** * @brief Remove predicates that reference a specific instance. * @@ -275,8 +333,7 @@ class ProblemExpert : public ProblemExpertInterface * @param[in] instance The instance to check for references. */ void removeInvalidPredicates( - std::vector & predicates, - const plansys2::Instance & instance); + std::vector & predicates, const plansys2::Instance & instance); /** * @brief Remove functions that reference a specific instance. @@ -285,8 +342,7 @@ class ProblemExpert : public ProblemExpertInterface * @param[in] instance The instance to check for references. */ void removeInvalidFunctions( - std::vector & functions, - const plansys2::Instance & instance); + std::vector & functions, const plansys2::Instance & instance); /** * @brief Remove goals that reference a specific instance. @@ -295,9 +351,7 @@ class ProblemExpert : public ProblemExpertInterface */ void removeInvalidGoals(const plansys2::Instance & instance); - std::vector instances_; - std::vector predicates_; - std::vector functions_; + plansys2::State state_; plansys2::Goal goal_; std::shared_ptr domain_expert_; diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp index 0d330ed7e..f9e25cd45 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp @@ -18,29 +18,31 @@ #include #include #include +#include #include -#include "plansys2_problem_expert/ProblemExpertInterface.hpp" -#include "plansys2_core/Types.hpp" - -#include "std_msgs/msg/empty.hpp" - +#include "plansys2_msgs/msg/node.hpp" +#include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/problem.hpp" +#include "plansys2_msgs/msg/tree.hpp" #include "plansys2_msgs/srv/add_problem.hpp" #include "plansys2_msgs/srv/add_problem_goal.hpp" #include "plansys2_msgs/srv/affect_node.hpp" +#include "plansys2_msgs/srv/affect_nodes.hpp" #include "plansys2_msgs/srv/affect_param.hpp" +#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" #include "plansys2_msgs/srv/exist_node.hpp" +#include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_problem.hpp" #include "plansys2_msgs/srv/get_problem_goal.hpp" #include "plansys2_msgs/srv/get_problem_instance_details.hpp" #include "plansys2_msgs/srv/get_problem_instances.hpp" -#include "plansys2_msgs/srv/get_node_details.hpp" +#include "plansys2_msgs/srv/get_problem_state.hpp" #include "plansys2_msgs/srv/get_states.hpp" #include "plansys2_msgs/srv/is_problem_goal_satisfied.hpp" #include "plansys2_msgs/srv/remove_problem_goal.hpp" -#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" - +#include "plansys2_msgs/srv/update_nodes.hpp" +#include "plansys2_problem_expert/ProblemExpertInterface.hpp" #include "rclcpp/rclcpp.hpp" namespace plansys2 @@ -68,9 +70,9 @@ class ProblemExpertClient : public ProblemExpertInterface /** * @brief Get all instances in the problem. * - * @return std::vector Vector containing all instances in the problem. + * @return std::unordered_set Set containing all instances in the problem. */ - std::vector getInstances(); + std::unordered_set getInstances(); /** * @brief Add a new instance to the problem. @@ -99,9 +101,16 @@ class ProblemExpertClient : public ProblemExpertInterface /** * @brief Get all predicates in the problem. * - * @return std::vector Vector containing all predicates in the problem. + * @return std::unordered_set Set containing all predicates in the problem. */ - std::vector getPredicates(); + std::unordered_set getPredicates(); + + /** + * @brief Get all inferred predicates in the problem. + * + * @return std::unordered_set Set containing all inferred predicates in the problem. + */ + std::unordered_set getInferredPredicates(); /** * @brief Add a new predicate to the problem. @@ -111,6 +120,14 @@ class ProblemExpertClient : public ProblemExpertInterface */ bool addPredicate(const plansys2::Predicate & predicate); + /** + * @brief Add new predicates to the problem. + * + * @param[in] predicates The predicates to be added. + * @return true if the predicates were successfully added, false otherwise. + */ + bool addPredicates(const std::vector & predicates); + /** * @brief Remove a predicate from the problem. * @@ -119,6 +136,25 @@ class ProblemExpertClient : public ProblemExpertInterface */ bool removePredicate(const plansys2::Predicate & predicate); + /** + * @brief Remove multiple predicates from the problem. + * + * @param[in] predicates The predicates to be removed. + * @return true if the predicates were successfully removed, false otherwise. + */ + bool removePredicates(const std::vector & predicates); + + /** + * @brief Update predicates in the problem. + * + * @param[in] add_predicates The predicates to be added. + * @param[in] remove_predicates The predicates to be removed. + * @return true if the predicates were successfully updated, false otherwise. + */ + bool updatePredicates( + const std::vector & add_predicates, + const std::vector & remove_predicates); + /** * @brief Check if a predicate exists in the problem. * @@ -138,9 +174,9 @@ class ProblemExpertClient : public ProblemExpertInterface /** * @brief Get all functions in the problem. * - * @return std::vector Vector containing all functions in the problem. + * @return std::unordered_set Set containing all functions in the problem. */ - std::vector getFunctions(); + std::unordered_set getFunctions(); /** * @brief Add a new function to the problem. @@ -182,6 +218,13 @@ class ProblemExpertClient : public ProblemExpertInterface */ std::optional getFunction(const std::string & function); + /** + * @brief Get the current state of the problem. + * + * @return plansys2::State The current state of the problem. + */ + plansys2::State getState(); + /** * @brief Get the current goal of the problem. * @@ -261,48 +304,36 @@ class ProblemExpertClient : public ProblemExpertInterface rclcpp::Time problem_ts_; private: - rclcpp::Client::SharedPtr - add_problem_client_; - rclcpp::Client::SharedPtr - add_problem_goal_client_; - rclcpp::Client::SharedPtr - add_problem_instance_client_; - rclcpp::Client::SharedPtr - add_problem_predicate_client_; - rclcpp::Client::SharedPtr - add_problem_function_client_; - rclcpp::Client::SharedPtr - get_problem_goal_client_; + rclcpp::Client::SharedPtr add_problem_client_; + rclcpp::Client::SharedPtr add_problem_goal_client_; + rclcpp::Client::SharedPtr add_problem_instance_client_; + rclcpp::Client::SharedPtr add_problem_predicate_client_; + rclcpp::Client::SharedPtr add_problem_predicates_client_; + rclcpp::Client::SharedPtr update_problem_predicates_client_; + rclcpp::Client::SharedPtr add_problem_function_client_; + rclcpp::Client::SharedPtr get_problem_goal_client_; + rclcpp::Client::SharedPtr get_problem_state_client_; rclcpp::Client::SharedPtr get_problem_instance_details_client_; - rclcpp::Client::SharedPtr - get_problem_instances_client_; + rclcpp::Client::SharedPtr get_problem_instances_client_; rclcpp::Client::SharedPtr get_problem_predicate_details_client_; - rclcpp::Client::SharedPtr - get_problem_predicates_client_; + rclcpp::Client::SharedPtr get_problem_predicates_client_; + rclcpp::Client::SharedPtr get_problem_inferred_predicates_client_; rclcpp::Client::SharedPtr get_problem_function_details_client_; - rclcpp::Client::SharedPtr - get_problem_functions_client_; - rclcpp::Client::SharedPtr - get_problem_client_; - rclcpp::Client::SharedPtr - remove_problem_goal_client_; + rclcpp::Client::SharedPtr get_problem_functions_client_; + rclcpp::Client::SharedPtr get_problem_client_; + rclcpp::Client::SharedPtr remove_problem_goal_client_; rclcpp::Client::SharedPtr clear_problem_knowledge_client_; - rclcpp::Client::SharedPtr - remove_problem_instance_client_; - rclcpp::Client::SharedPtr - remove_problem_predicate_client_; - rclcpp::Client::SharedPtr - remove_problem_function_client_; - rclcpp::Client::SharedPtr - exist_problem_predicate_client_; - rclcpp::Client::SharedPtr - exist_problem_function_client_; - rclcpp::Client::SharedPtr - update_problem_function_client_; + rclcpp::Client::SharedPtr remove_problem_instance_client_; + rclcpp::Client::SharedPtr remove_problem_predicate_client_; + rclcpp::Client::SharedPtr remove_problem_predicates_client_; + rclcpp::Client::SharedPtr remove_problem_function_client_; + rclcpp::Client::SharedPtr exist_problem_predicate_client_; + rclcpp::Client::SharedPtr exist_problem_function_client_; + rclcpp::Client::SharedPtr update_problem_function_client_; rclcpp::Client::SharedPtr is_problem_goal_satisfied_client_; rclcpp::Subscription::SharedPtr problem_sub_; diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp index 572216c62..c39bf0934 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp @@ -15,11 +15,16 @@ #ifndef PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERTINTERFACE_HPP_ #define PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERTINTERFACE_HPP_ -#include -#include #include +#include +#include +#include "plansys2_core/DerivedResolutionGraph.hpp" +#include "plansys2_core/State.hpp" #include "plansys2_core/Types.hpp" +#include "plansys2_msgs/msg/node.hpp" +#include "plansys2_msgs/msg/param.hpp" +#include "plansys2_msgs/msg/tree.hpp" namespace plansys2 { @@ -42,9 +47,9 @@ class ProblemExpertInterface /** * @brief Get all instances in the problem. * - * @return std::vector Vector containing all instances in the problem. + * @return std::unordered_set Set containing all instances in the problem. */ - virtual std::vector getInstances() = 0; + virtual std::unordered_set getInstances() = 0; /** * @brief Add a new instance to the problem. @@ -73,9 +78,16 @@ class ProblemExpertInterface /** * @brief Get all predicates in the problem. * - * @return std::vector Vector containing all predicates in the problem. + * @return std::unordered_set Set containing all predicates in the problem. + */ + virtual std::unordered_set getPredicates() = 0; + + /** + * @brief Get all inferred predicates in the problem. + * + * @return std::unordered_set Set containing all inferred predicates in the problem. */ - virtual std::vector getPredicates() = 0; + virtual std::unordered_set getInferredPredicates() = 0; /** * @brief Add a new predicate to the problem. @@ -85,6 +97,14 @@ class ProblemExpertInterface */ virtual bool addPredicate(const plansys2::Predicate & predicate) = 0; + /** + * @brief Add new predicates to the problem. + * + * @param[in] predicates The predicates to be added. + * @return true if the predicates were successfully added, false otherwise. + */ + virtual bool addPredicates(const std::vector & predicates) = 0; + /** * @brief Remove a predicate from the problem. * @@ -93,6 +113,25 @@ class ProblemExpertInterface */ virtual bool removePredicate(const plansys2::Predicate & predicate) = 0; + /** + * @brief Remove multiple predicates from the problem. + * + * @param[in] predicates The predicates to be removed. + * @return true if the predicates were successfully removed, false otherwise. + */ + virtual bool removePredicates(const std::vector & predicates) = 0; + + /** + * @brief Update predicates in the problem. + * + * @param[in] add_predicates The predicates to be added. + * @param[in] remove_predicates The predicates to be removed. + * @return true if the predicates were successfully updated, false otherwise. + */ + virtual bool updatePredicates( + const std::vector & add_predicates, + const std::vector & remove_predicates) = 0; + /** * @brief Check if a predicate exists in the problem. * @@ -112,9 +151,9 @@ class ProblemExpertInterface /** * @brief Get all functions in the problem. * - * @return std::vector Vector containing all functions in the problem. + * @return std::unordered_set Set containing all functions in the problem. */ - virtual std::vector getFunctions() = 0; + virtual std::unordered_set getFunctions() = 0; /** * @brief Add a new function to the problem. @@ -156,6 +195,13 @@ class ProblemExpertInterface */ virtual std::optional getFunction(const std::string & expr) = 0; + /** + * @brief Get the current state of the problem. + * + * @return plansys2::State The current state of the problem. + */ + virtual plansys2::State getState() = 0; + /** * @brief Get the current goal of the problem. * diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp index 47379b3b7..ac7aa59d4 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp @@ -17,28 +17,32 @@ #include -#include "plansys2_problem_expert/ProblemExpert.hpp" - -#include "std_msgs/msg/empty.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" #include "plansys2_msgs/msg/knowledge.hpp" #include "plansys2_msgs/msg/problem.hpp" -#include "plansys2_msgs/srv/affect_node.hpp" -#include "plansys2_msgs/srv/affect_param.hpp" #include "plansys2_msgs/srv/add_problem.hpp" #include "plansys2_msgs/srv/add_problem_goal.hpp" +#include "plansys2_msgs/srv/affect_node.hpp" +#include "plansys2_msgs/srv/affect_nodes.hpp" +#include "plansys2_msgs/srv/affect_param.hpp" +#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" #include "plansys2_msgs/srv/exist_node.hpp" +#include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_problem.hpp" #include "plansys2_msgs/srv/get_problem_goal.hpp" #include "plansys2_msgs/srv/get_problem_instance_details.hpp" #include "plansys2_msgs/srv/get_problem_instances.hpp" -#include "plansys2_msgs/srv/get_node_details.hpp" +#include "plansys2_msgs/srv/get_problem_state.hpp" #include "plansys2_msgs/srv/get_states.hpp" #include "plansys2_msgs/srv/is_problem_goal_satisfied.hpp" #include "plansys2_msgs/srv/remove_problem_goal.hpp" -#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" - +#include "plansys2_msgs/srv/update_nodes.hpp" +#include "plansys2_problem_expert/ProblemExpert.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/empty.hpp" +#include "std_msgs/msg/string.hpp" namespace plansys2 { @@ -61,8 +65,7 @@ class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode */ ProblemExpertNode(); - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /** * @brief Configures the node. @@ -168,6 +171,16 @@ class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode const std::shared_ptr request, const std::shared_ptr response); + void add_problem_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void update_problem_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + /** * @brief Service callback to add a problem function. * @@ -192,6 +205,18 @@ class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode const std::shared_ptr request, const std::shared_ptr response); + /** + * @brief Service callback to get the problem state. + * + * @param[in] request_header ROS service request header. + * @param[in] request Empty service request. + * @param[out] response Service response containing the current state and success status. + */ + void get_problem_state_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + /** * @brief Service callback to get details about a specific problem instance. * @@ -240,6 +265,18 @@ class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode const std::shared_ptr request, const std::shared_ptr response); + /** + * @brief Service callback to get all problem inferred predicates. + * + * @param[in] request_header ROS service request header. + * @param[in] request Empty service request. + * @param[out] response Service response containing the list of inferred predicates. + */ + void get_problem_inferred_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + /** * @brief Service callback to get details about a specific problem function. * @@ -336,6 +373,18 @@ class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode const std::shared_ptr request, const std::shared_ptr response); + /** + * @brief Service callback to remove a list of predicates. + * + * @param[in] request_header ROS service request header. + * @param[in] request Service request containing the predicates node to remove. + * @param[out] response Service response with success status and error information. + */ + void remove_problem_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + /** * @brief Service callback to remove a problem function. * @@ -388,50 +437,40 @@ class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr problem_expert_; // Service servers - rclcpp::Service::SharedPtr - add_problem_service_; - rclcpp::Service::SharedPtr - add_problem_goal_service_; - rclcpp::Service::SharedPtr - add_problem_instance_service_; - rclcpp::Service::SharedPtr - add_problem_predicate_service_; - rclcpp::Service::SharedPtr - add_problem_function_service_; - rclcpp::Service::SharedPtr - get_problem_goal_service_; + rclcpp::Service::SharedPtr add_problem_service_; + rclcpp::Service::SharedPtr add_problem_goal_service_; + rclcpp::Service::SharedPtr add_problem_instance_service_; + rclcpp::Service::SharedPtr add_problem_predicate_service_; + rclcpp::Service::SharedPtr add_problem_predicates_service_; + rclcpp::Service::SharedPtr update_problem_predicates_service_; + rclcpp::Service::SharedPtr add_problem_function_service_; + rclcpp::Service::SharedPtr get_problem_goal_service_; + rclcpp::Service::SharedPtr get_problem_state_service_; rclcpp::Service::SharedPtr get_problem_instance_details_service_; rclcpp::Service::SharedPtr get_problem_instances_service_; rclcpp::Service::SharedPtr get_problem_predicate_details_service_; + rclcpp::Service::SharedPtr get_problem_predicates_service_; rclcpp::Service::SharedPtr - get_problem_predicates_service_; + get_problem_inferred_predicates_service_; rclcpp::Service::SharedPtr get_problem_function_details_service_; - rclcpp::Service::SharedPtr - get_problem_functions_service_; - rclcpp::Service::SharedPtr - get_problem_service_; + rclcpp::Service::SharedPtr get_problem_functions_service_; + rclcpp::Service::SharedPtr get_problem_service_; rclcpp::Service::SharedPtr is_problem_goal_satisfied_service_; - rclcpp::Service::SharedPtr - remove_problem_goal_service_; + rclcpp::Service::SharedPtr remove_problem_goal_service_; rclcpp::Service::SharedPtr clear_problem_knowledge_service_; - rclcpp::Service::SharedPtr - remove_problem_instance_service_; - rclcpp::Service::SharedPtr - remove_problem_predicate_service_; - rclcpp::Service::SharedPtr - remove_problem_function_service_; - rclcpp::Service::SharedPtr - exist_problem_predicate_service_; - rclcpp::Service::SharedPtr - exist_problem_function_service_; - rclcpp::Service::SharedPtr - update_problem_function_service_; + rclcpp::Service::SharedPtr remove_problem_instance_service_; + rclcpp::Service::SharedPtr remove_problem_predicate_service_; + rclcpp::Service::SharedPtr remove_problem_predicates_service_; + rclcpp::Service::SharedPtr remove_problem_function_service_; + rclcpp::Service::SharedPtr exist_problem_predicate_service_; + rclcpp::Service::SharedPtr exist_problem_function_service_; + rclcpp::Service::SharedPtr update_problem_function_service_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr update_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr knowledge_pub_; diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp index ee255fc24..9a3a89d07 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp @@ -15,79 +15,100 @@ #ifndef PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_ #define PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_ -#include -#include -#include #include -#include +#include #include +#include +#include +#include #include +#include -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_msgs/msg/tree.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" namespace plansys2 { +std::tuple>> unifyPredicate( + const plansys2::Predicate & predicate, + const std::unordered_set & predicates); + +std::tuple>> unifyFunction( + const plansys2::Function & function, const std::unordered_set & functions); + +void mergeParamsValuesDicts( + const std::map & dict1, + const std::map & dict2, std::map & dict3); +std::vector> mergeParamsValuesVector( + const std::vector> & vector1, + const std::vector> & vector2); +std::vector> complementParamsValuesVector( + const std::vector & params, + const std::vector> & param_dict_vector, + const std::unordered_set & instances); + +std::tuple>> negateResult( + const plansys2_msgs::msg::Node & node, const bool & result, + const std::vector> & param_dict_vector, + const std::unordered_set & instances); + +std::tuple>> negateResult( + const std::vector & params, const bool & result, + const std::vector> & param_dict_vector, + const std::unordered_set & instances); + +std::vector get_node_children_free_parameters( + const plansys2_msgs::msg::Tree & tree, const plansys2_msgs::msg::Node & current_node); +void get_node_children_free_parameters_impl( + const plansys2_msgs::msg::Tree & tree, const plansys2_msgs::msg::Node & current_node, + std::vector & params, std::unordered_set & seen, + std::unordered_set & exists_params); + +std::vector get_node_free_parameters( + const plansys2_msgs::msg::Node & node); +void get_node_free_parameters_impl( + const plansys2_msgs::msg::Node & node, std::vector & params, + std::unordered_set & seen); + +void solveDerivedPredicates( + plansys2::State & state, const std::vector & root_nodes); + +void solveDerivedPredicates(plansys2::State & state); +bool evaluateSCC( + const std::vector & scc, plansys2::State & state, + const std::vector & root_nodes, + std::unordered_set & unground_cache); + +void groundPredicate( + plansys2::State & new_state, const plansys2::Derived & derived, + const std::vector> & params_values_vector); + +/// Evaluate a PDDL expression represented as a tree. /** - * @brief Evaluate a PDDL expression represented as a tree. - * - * @param[in] tree The root node of the PDDL expression. - * @param[in] problem_client The problem expert client. - * @param[in,out] predicates Current predicates state. - * @param[in,out] functions Current functions state. - * @param[in] apply Apply result to problem expert or state (default: false). - * @param[in] use_state Use state representation or problem client (default: false). - * @param[in] node_id Node identifier in the tree (default: 0). - * @param[in] negate Invert the truth value of the expression (default: false). - * @return tuple(bool, bool, double) with execution result in this format: - * result <- tuple(bool, bool, double) + * \param[in] node The root node of the PDDL expression. + * \param[in] problem_client The problem expert client. + * \param[in] instances Current instances state. + * \param[in] predicates Current predicates state. + * \param[in] functions Current functions state. + * \param[in] apply Apply result to problem expert or state. + * \param[in] use_state Use state representation or problem client. + * \param[in] negate Invert the truth value. + * \return result <- tuple(bool, bool, double) * result(0) true if success * result(1) truth value of boolean expression * result(2) value of numeric expression + * result(3) vector with the set of possible values for the expression parameters */ -std::tuple evaluate( - const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - std::vector & predicates, - std::vector & functions, - bool apply = false, - bool use_state = false, - uint8_t node_id = 0, +std::tuple>> evaluate( + const plansys2_msgs::msg::Tree & tree, const plansys2::State & state, uint8_t node_id = 0, bool negate = false); -/** - * @brief Evaluate a PDDL expression represented as a tree using only the problem client. - * - * @param[in] tree The root node of the PDDL expression. - * @param[in] problem_client The problem expert client. - * @param[in] apply Apply result to problem expert (default: false). - * @param[in] node_id Node identifier in the tree. - * @return tuple(bool, bool, double) with execution result. - */ -std::tuple evaluate( +std::tuple>> evaluate( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - bool apply = false, - uint32_t node_id = 0); - -/** - * @brief Evaluate a PDDL expression represented as a tree using only local state. - * - * @param[in] tree The root node of the PDDL expression. - * @param[in,out] predicates Current predicates state. - * @param[in,out] functions Current functions state. - * @param[in] apply Apply result to state (default: false). - * @param[in] node_id Node identifier in the tree (default: 0). - * @return tuple(bool, bool, double) with execution result. - */ -std::tuple evaluate( - const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - bool apply = false, - uint32_t node_id = 0); + std::shared_ptr problem_client, uint32_t node_id = 0, + bool negate = false); /** * @brief Check a PDDL expression represented as a tree. @@ -100,8 +121,8 @@ std::tuple evaluate( */ bool check( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - uint32_t node_id = 0); + std::shared_ptr problem_client, uint32_t node_id = 0, + bool negate = false); /** * @brief Check a PDDL expression represented as a tree using local state. @@ -113,10 +134,8 @@ bool check( * @return bool Truth value of the PDDL expression. */ bool check( - const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - uint32_t node_id = 0); + const plansys2_msgs::msg::Tree & tree, const plansys2::State & state, uint32_t node_id = 0, + bool negate = false); /** * @brief Apply a PDDL expression represented as a tree. @@ -129,8 +148,17 @@ bool check( */ bool apply( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - uint32_t node_id = 0); + std::shared_ptr problem_client, uint32_t node_id = 0, + bool negate = false, bool derive = true); + +bool apply( + const plansys2_msgs::msg::Tree & tree, plansys2::State & state, uint32_t node_id = 0, + bool negate = false, bool derive = true); + +bool apply( + const plansys2_msgs::msg::Tree & tree, plansys2::State & state, + std::vector & nodes_modified, uint32_t node_id = 0, bool negate = false, + bool derive = true); /** * @brief Apply a PDDL expression represented as a tree using local state. @@ -144,9 +172,9 @@ bool apply( */ bool apply( const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - uint32_t node_id = 0); + std::shared_ptr problem_client, plansys2::State & state, + std::vector & nodes_modified, bool use_state = false, + uint32_t node_id = 0, bool negate = false, bool derive = true); /** * @brief Parse the action expression and time (optional) from an input string. @@ -202,41 +230,6 @@ std::string get_action_name(const std::string & input); */ std::vector get_action_params(const std::string & action_expr); -/** - * @brief Replace parameter names in children nodes of a PDDL expression tree. - * This function creates a new tree and recursively replaces parameter names - * in the current node and all its children according to the replacement map. - * It's commonly used in quantified expressions (exists, forall) to substitute - * parameters with specific values. - * - * @param[in] tree The PDDL expression tree. - * @param[in] node_id The identifier of the node to start replacement. - * @param[in] replace Map of original parameter names to replacement names. - * @return plansys2_msgs::msg::Tree The modified tree with replaced parameter names. - */ -plansys2_msgs::msg::Tree replace_children_param( - const plansys2_msgs::msg::Tree & tree, - const uint8_t & node_id, - const std::map & replace); - -/** - * @brief Compute the Cartesian product of vectors of strings. - * This function recursively computes all possible combinations of elements - * from multiple vectors. For example, given vectors [a,b] and [1,2], - * it generates combinations [a,1], [a,2], [b,1], [b,2]. - * It's used primarily for expanding quantified expressions in PDDL. - * - * @param[out] rvvi Result vector containing all combinations. - * @param[out] rvi Current combination being built. - * @param[in] me Iterator to the current vector in the input. - * @param[in] end Iterator to the end of the input vectors. - */ -void cart_product( - std::vector> & rvvi, - std::vector & rvi, - std::vector>::const_iterator me, - std::vector>::const_iterator end); } // namespace plansys2 - #endif // PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_ diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp index 54d746eaf..516498f5c 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp @@ -14,32 +14,36 @@ #include "plansys2_problem_expert/ProblemExpert.hpp" -#include +#include // OpenMP for parallelization + #include +#include +#include +#include +#include #include #include #include -#include -#include -#include #include "plansys2_core/Utils.hpp" #include "plansys2_pddl_parser/Domain.hpp" #include "plansys2_pddl_parser/Instance.hpp" #include "plansys2_problem_expert/Utils.hpp" -#include "plansys2_core/Types.hpp" - namespace plansys2 { ProblemExpert::ProblemExpert(std::shared_ptr & domain_expert) : domain_expert_(domain_expert) { + state_.setDerivedPredicates(domain_expert_->getDerivedResolutionGraph()); } -bool -ProblemExpert::addInstance(const plansys2::Instance & instance) +void ProblemExpert::updateInferredPredicates() {solveDerivedPredicates(state_);} + +plansys2::State ProblemExpert::getState() {return state_;} + +bool ProblemExpert::addInstance(const plansys2::Instance & instance) { plansys2::Instance lowercase_instance = instance; std::transform( @@ -63,182 +67,110 @@ ProblemExpert::addInstance(const plansys2::Instance & instance) } if (!exist_instance) { - instances_.push_back(lowercase_instance); + state_.addInstance(lowercase_instance); } return true; } -std::vector -ProblemExpert::getInstances() +std::unordered_set ProblemExpert::getInstances() { - return instances_; + return state_.getInstances(); } -bool -ProblemExpert::removeInstance(const plansys2::Instance & instance) +bool ProblemExpert::removeInstance(const plansys2::Instance & instance) { - bool found = false; - int i = 0; - - while (!found && i < instances_.size()) { - if (instances_[i].name == instance.name) { - found = true; - instances_.erase(instances_.begin() + i); - } - i++; - } - - removeInvalidPredicates(predicates_, instance); - removeInvalidFunctions(functions_, instance); + auto res = state_.removeInstance(instance); + removeInvalidPredicates(instance); + removeInvalidFunctions(instance); removeInvalidGoals(instance); - return found; + return res; } -std::optional -ProblemExpert::getInstance(const std::string & instance_name) +std::optional ProblemExpert::getInstance(const std::string & instance_name) { - plansys2::Instance ret; - - bool found = false; - int i = 0; - while (i < instances_.size() && !found) { - if (instances_[i].name == instance_name) { - found = true; - ret = instances_[i]; - } - i++; - } - - if (found) { - return ret; - } else { - return {}; + auto it = state_.getInstance(parser::pddl::fromStringParam(instance_name)); + if (it != state_.getInstances().end()) { + return *it; } + return {}; } -std::vector -ProblemExpert::getPredicates() +std::unordered_set ProblemExpert::getPredicates() { - std::vector ret = predicates_; + return state_.getPredicates(); +} - auto derived_predicates = domain_expert_->getDerivedPredicates(); - for (auto derived_name : derived_predicates) { - auto derived = domain_expert_->getDerivedPredicate(derived_name.predicate.name); - for (auto d : derived) { - std::vector> parameters_vector; - for (size_t i = 0; i < d.predicate.parameters.size(); i++) { - std::vector p_vector; - std::for_each( - instances_.begin(), instances_.end(), - [&](auto instance) { - if (d.predicate.parameters[i].type == instance.type) { - p_vector.push_back(instance.name); - } - }); - parameters_vector.push_back(p_vector); - } - std::vector> possible_parameters_values; - std::vector aux; - plansys2::cart_product( - possible_parameters_values, aux, parameters_vector.begin(), parameters_vector.end()); - - for (auto parameters_values : possible_parameters_values) { - std::map replace; - for (size_t i = 0; i < d.predicate.parameters.size(); i++) { - replace["?" + std::to_string(i)] = parameters_values[i]; - } - auto tree_replaced = plansys2::replace_children_param( - d.preconditions, d.preconditions.nodes[0].node_id, replace); - bool result = check(tree_replaced, predicates_, functions_); - if (result) { - plansys2::Predicate inferred_predicate; - inferred_predicate.node_type = plansys2_msgs::msg::Node::PREDICATE; - inferred_predicate.name = d.predicate.name; - for (size_t i = 0; i < d.predicate.parameters.size(); i++) { - plansys2_msgs::msg::Param param; - param.name = replace.at("?" + std::to_string(i)); - inferred_predicate.parameters.push_back(param); - } - ret.push_back(inferred_predicate); - } - } - } - } - return ret; +std::unordered_set ProblemExpert::getInferredPredicates() +{ + updateInferredPredicates(); + return state_.getInferredPredicates(); } -bool -ProblemExpert::addPredicate(const plansys2::Predicate & predicate) +bool ProblemExpert::addPredicate(const plansys2::Predicate & predicate) { - if (!existPredicate(predicate)) { - if (isValidPredicate(predicate)) { - predicates_.push_back(predicate); - return true; - } else { - return false; - } - } else { + if (state_.hasPredicate(predicate)) { return true; } + if (isValidPredicate(predicate)) { + return state_.addPredicate(predicate); + } + return false; } -bool -ProblemExpert::removePredicate(const plansys2::Predicate & predicate) +bool ProblemExpert::addPredicates(const std::vector & predicates) { - bool found = false; - int i = 0; + bool result = true; + for (const auto & predicate : predicates) { + result &= addPredicate(predicate); + } + return result; +} +bool ProblemExpert::removePredicate(const plansys2::Predicate & predicate) +{ if (!isValidPredicate(predicate)) { // if predicate is not valid, error return false; } - while (!found && i < predicates_.size()) { - if (parser::pddl::checkNodeEquality(predicates_[i], predicate)) { - found = true; - predicates_.erase(predicates_.begin() + i); - } - i++; - } - - return true; + return state_.removePredicate(predicate); } -std::optional -ProblemExpert::getPredicate(const std::string & expr) +bool ProblemExpert::removePredicates(const std::vector & predicates) { - plansys2::Predicate ret; - plansys2::Predicate pred = parser::pddl::fromStringPredicate(expr); - - bool found = false; - size_t i = 0; - while (i < predicates_.size() && !found) { - if (parser::pddl::checkNodeEquality(predicates_[i], pred)) { - found = true; - ret = predicates_[i]; - } - i++; + bool result = true; + for (const auto & predicate : predicates) { + result &= removePredicate(predicate); } + return result; +} - if (found) { - return ret; - } else { - return {}; +bool ProblemExpert::updatePredicates( + const std::vector & add_predicates, + const std::vector & remove_predicates) +{ + return removePredicates(remove_predicates) && addPredicates(add_predicates); +} + +std::optional ProblemExpert::getPredicate(const std::string & expr) +{ + auto it = state_.getPredicate(parser::pddl::fromStringPredicate(expr)); + if (it != state_.getPredicates().end()) { + return *it; } + return {}; } -std::vector -ProblemExpert::getFunctions() +std::unordered_set ProblemExpert::getFunctions() { - return functions_; + return state_.getFunctions(); } -bool -ProblemExpert::addFunction(const plansys2::Function & function) +bool ProblemExpert::addFunction(const plansys2::Function & function) { if (!existFunction(function)) { if (isValidFunction(function)) { - functions_.push_back(function); + state_.addFunction(function); return true; } else { return false; @@ -248,33 +180,17 @@ ProblemExpert::addFunction(const plansys2::Function & function) } } -bool -ProblemExpert::removeFunction(const plansys2::Function & function) +bool ProblemExpert::removeFunction(const plansys2::Function & function) { - bool found = false; - int i = 0; - - if (!isValidFunction(function)) { // if function is not valid, error - return false; - } - while (!found && i < functions_.size()) { - if (parser::pddl::checkNodeEquality(functions_[i], function)) { - found = true; - functions_.erase(functions_.begin() + i); - } - i++; - } - - return true; + return state_.removeFunction(function); } -bool -ProblemExpert::updateFunction(const plansys2::Function & function) +bool ProblemExpert::updateFunction(const plansys2::Function & function) { if (existFunction(function)) { if (isValidFunction(function)) { removeFunction(function); - functions_.push_back(function); + state_.addFunction(function); return true; } else { return false; @@ -284,59 +200,43 @@ ProblemExpert::updateFunction(const plansys2::Function & function) } } -std::optional -ProblemExpert::getFunction(const std::string & expr) +std::optional ProblemExpert::getFunction(const std::string & expr) { - plansys2::Function ret; - plansys2::Function func = parser::pddl::fromStringFunction(expr); - - bool found = false; - size_t i = 0; - while (i < functions_.size() && !found) { - if (parser::pddl::checkNodeEquality(functions_[i], func)) { - found = true; - ret = functions_[i]; - } - i++; - } - - if (found) { - return ret; - } else { - return {}; + auto it = state_.getFunction(parser::pddl::fromStringFunction(expr)); + if (it != state_.getFunctions().end()) { + return *it; } + return {}; } -void -ProblemExpert::removeInvalidPredicates( - std::vector & predicates, - const plansys2::Instance & instance) +void ProblemExpert::removeInvalidPredicates(const plansys2::Instance & instance) { - for (auto rit = predicates.rbegin(); rit != predicates.rend(); ++rit) { - if (std::find_if( - rit->parameters.begin(), rit->parameters.end(), - [&](const plansys2_msgs::msg::Param & param) { + for (auto it = state_.getPredicates().begin(); it != state_.getPredicates().end(); ) { + if ( + std::find_if( + it->parameters.begin(), it->parameters.end(), [&](const plansys2_msgs::msg::Param & param) { return param.name == instance.name; - }) != rit->parameters.end()) + }) != it->parameters.end()) { - predicates.erase(std::next(rit).base()); + it = state_.removePredicate(it); + } else { + ++it; } } } -void -ProblemExpert::removeInvalidFunctions( - std::vector & functions, - const plansys2::Instance & instance) +void ProblemExpert::removeInvalidFunctions(const plansys2::Instance & instance) { - for (auto rit = functions.rbegin(); rit != functions.rend(); ++rit) { - if (std::find_if( - rit->parameters.begin(), rit->parameters.end(), - [&](const plansys2_msgs::msg::Param & param) { + for (auto it = state_.getFunctions().begin(); it != state_.getFunctions().end(); ) { + if ( + std::find_if( + it->parameters.begin(), it->parameters.end(), [&](const plansys2_msgs::msg::Param & param) { return param.name == instance.name; - }) != rit->parameters.end()) + }) != it->parameters.end()) { - functions.erase(std::next(rit).base()); + it = state_.removeFunction(it); + } else { + ++it; } } } @@ -360,11 +260,11 @@ void ProblemExpert::removeInvalidGoals(const plansys2::Instance & instance) // Check predicates for removed instance. bool params_valid = true; for (const auto & predicate : predicates) { - if (std::find_if( + if ( + std::find_if( predicate.parameters.begin(), predicate.parameters.end(), - [&](const plansys2_msgs::msg::Param & param) { - return param.name == instance.name; - }) != predicate.parameters.end()) + [&](const plansys2_msgs::msg::Param & param) {return param.name == instance.name;}) != + predicate.parameters.end()) { params_valid = false; break; @@ -384,11 +284,11 @@ void ProblemExpert::removeInvalidGoals(const plansys2::Instance & instance) // Check functions for removed instance. params_valid = true; for (const auto & function : functions) { - if (std::find_if( + if ( + std::find_if( function.parameters.begin(), function.parameters.end(), - [&](const plansys2_msgs::msg::Param & param) { - return param.name == instance.name; - }) != function.parameters.end()) + [&](const plansys2_msgs::msg::Param & param) {return param.name == instance.name;}) != + function.parameters.end()) { params_valid = false; break; @@ -410,14 +310,9 @@ void ProblemExpert::removeInvalidGoals(const plansys2::Instance & instance) } } -plansys2::Goal -ProblemExpert::getGoal() -{ - return goal_; -} +plansys2::Goal ProblemExpert::getGoal() {return goal_;} -bool -ProblemExpert::setGoal(const plansys2::Goal & goal) +bool ProblemExpert::setGoal(const plansys2::Goal & goal) { if (isValidGoal(goal)) { goal_ = goal; @@ -429,29 +324,25 @@ ProblemExpert::setGoal(const plansys2::Goal & goal) bool ProblemExpert::isGoalSatisfied(const plansys2::Goal & goal) { - return check(goal, predicates_, functions_); + updateInferredPredicates(); + return check(goal, state_); } -bool -ProblemExpert::clearGoal() +bool ProblemExpert::clearGoal() { goal_.nodes.clear(); return true; } -bool -ProblemExpert::clearKnowledge() +bool ProblemExpert::clearKnowledge() { - instances_.clear(); - predicates_.clear(); - functions_.clear(); + state_.clearState(); clearGoal(); return true; } -bool -ProblemExpert::isValidType(const std::string & type) +bool ProblemExpert::isValidType(const std::string & type) { std::string lowercase_type = type; std::transform( @@ -464,70 +355,27 @@ ProblemExpert::isValidType(const std::string & type) return it != valid_types.end(); } -bool -ProblemExpert::existInstance(const std::string & name) +bool ProblemExpert::existInstance(const std::string & name) { - bool found = false; - int i = 0; - - while (!found && i < instances_.size()) { - if (instances_[i].name == name) { - found = true; - } - i++; - } - - return found; + return state_.hasInstance(parser::pddl::fromStringParam(name)); } -bool -ProblemExpert::existPredicate(const plansys2::Predicate & predicate) +bool ProblemExpert::existPredicate(const plansys2::Predicate & predicate) { - bool found = false; - int i = 0; - - while (!found && i < predicates_.size()) { - if (parser::pddl::checkNodeEquality(predicates_[i], predicate)) { - found = true; - } - i++; - } - - if (!found) { - std::vector parameters_names; - std::for_each( - predicate.parameters.begin(), predicate.parameters.end(), - [&](auto p) {parameters_names.push_back(p.name);}); - auto derived_predicates = domain_expert_->getDerivedPredicate(predicate.name, parameters_names); - for (auto derived : derived_predicates) { - if (check(derived.preconditions, predicates_, functions_)) { - found = true; - break; - } - } - } - - return found; + return state_.hasPredicate(predicate); } -bool -ProblemExpert::existFunction(const plansys2::Function & function) +bool ProblemExpert::existInferredPredicate(const plansys2::Predicate & predicate) { - bool found = false; - int i = 0; - - while (!found && i < functions_.size()) { - if (parser::pddl::checkNodeEquality(functions_[i], function)) { - found = true; - } - i++; - } + return state_.hasInferredPredicate(predicate); +} - return found; +bool ProblemExpert::existFunction(const plansys2::Function & function) +{ + return state_.hasFunction(function); } -bool -ProblemExpert::isValidPredicate(const plansys2::Predicate & predicate) +bool ProblemExpert::isValidPredicate(const plansys2::Predicate & predicate) { bool valid = false; @@ -571,8 +419,7 @@ ProblemExpert::isValidPredicate(const plansys2::Predicate & predicate) return valid; } -bool -ProblemExpert::isValidFunction(const plansys2::Function & function) +bool ProblemExpert::isValidFunction(const plansys2::Function & function) { bool valid = false; @@ -616,16 +463,13 @@ ProblemExpert::isValidFunction(const plansys2::Function & function) return valid; } -bool -ProblemExpert::isValidGoal(const plansys2::Goal & goal) +bool ProblemExpert::isValidGoal(const plansys2::Goal & goal) { return checkPredicateTreeTypes(goal, domain_expert_); } -bool -ProblemExpert::checkPredicateTreeTypes( - const plansys2_msgs::msg::Tree & tree, - std::shared_ptr & domain_expert, +bool ProblemExpert::checkPredicateTreeTypes( + const plansys2_msgs::msg::Tree & tree, std::shared_ptr & domain_expert, uint8_t node_id) { if (node_id >= tree.nodes.size()) { @@ -687,33 +531,32 @@ ProblemExpert::checkPredicateTreeTypes( default: // LCOV_EXCL_START - std::cerr << "checkPredicateTreeTypes: Error parsing expresion [" << - parser::pddl::toString(tree, node_id) << "]" << std::endl; + std::cerr << "checkPredicateTreeTypes: Error parsing expresion [" + << parser::pddl::toString(tree, node_id) << "]" << std::endl; // LCOV_EXCL_STOP } return false; } -std::string -ProblemExpert::getProblem() +std::string ProblemExpert::getProblem() { parser::pddl::Domain domain(domain_expert_->getDomain()); parser::pddl::Instance problem(domain); problem.name = "problem_1"; - for (const auto & instance : instances_) { + for (const auto & instance : state_.getInstances()) { bool is_constant = domain.getType(instance.type)->parseConstant(instance.name).first; if (is_constant) { - std::cout << "Skipping adding constant to problem :object: " << instance.name << " " << - instance.type << std::endl; + std::cout << "Skipping adding constant to problem :object: " << instance.name << " " + << instance.type << std::endl; } else { problem.addObject(instance.name, instance.type); } } - for (plansys2_msgs::msg::Node predicate : predicates_) { + for (plansys2_msgs::msg::Node predicate : state_.getPredicates()) { StringVec v; for (size_t i = 0; i < predicate.parameters.size(); i++) { @@ -725,16 +568,14 @@ ProblemExpert::getProblem() problem.addInit(predicate.name, v); } - for (plansys2_msgs::msg::Node function : functions_) { + for (plansys2_msgs::msg::Node function : state_.getFunctions()) { StringVec v; for (size_t i = 0; i < function.parameters.size(); i++) { v.push_back(function.parameters[i].name); } - std::transform( - function.name.begin(), function.name.end(), - function.name.begin(), ::tolower); + std::transform(function.name.begin(), function.name.end(), function.name.begin(), ::tolower); problem.addInit(function.name, function.value, v); } @@ -747,8 +588,7 @@ ProblemExpert::getProblem() return stream.str(); } -bool -ProblemExpert::addProblem(const std::string & problem_str) +bool ProblemExpert::addProblem(const std::string & problem_str) { if (problem_str.empty()) { std::cerr << "Empty problem." << std::endl; @@ -758,8 +598,9 @@ ProblemExpert::addProblem(const std::string & problem_str) std::string lc_problem = problem_str; std::transform( - problem_str.begin(), problem_str.end(), lc_problem.begin(), - [](unsigned char c) {return std::tolower(c);}); + problem_str.begin(), problem_str.end(), lc_problem.begin(), [](unsigned char c) { + return std::tolower(c); + }); lc_problem = remove_comments(lc_problem); @@ -789,7 +630,7 @@ ProblemExpert::addProblem(const std::string & problem_str) std::cout << "Parsed problem: " << problem << std::endl; for (unsigned i = 0; i < domain.types.size(); ++i) { - if (domain.types[i]->constants.size() ) { + if (domain.types[i]->constants.size()) { for (unsigned j = 0; j < domain.types[i]->constants.size(); ++j) { plansys2::Instance instance; instance.name = domain.types[i]->constants[j]; @@ -801,7 +642,7 @@ ProblemExpert::addProblem(const std::string & problem_str) } for (unsigned i = 0; i < domain.types.size(); ++i) { - if (domain.types[i]->objects.size() ) { + if (domain.types[i]->objects.size()) { for (unsigned j = 0; j < domain.types[i]->objects.size(); ++j) { plansys2::Instance instance; instance.name = domain.types[i]->objects[j]; @@ -818,28 +659,22 @@ ProblemExpert::addProblem(const std::string & problem_str) switch (tree_node->node_type) { case plansys2_msgs::msg::Node::PREDICATE: { plansys2::Predicate pred_node(*tree_node); - std::cout << "Adding predicate: " << - parser::pddl::toString(tree, tree_node->node_id) << std::endl; + std::cout << "Adding predicate: " << parser::pddl::toString(tree, tree_node->node_id) + << std::endl; if (!addPredicate(pred_node)) { - std::cerr << "Failed to add predicate: " << parser::pddl::toString( - tree, - tree_node->node_id) << - std::endl; + std::cerr << "Failed to add predicate: " + << parser::pddl::toString(tree, tree_node->node_id) << std::endl; } - } - break; + } break; case plansys2_msgs::msg::Node::FUNCTION: { plansys2::Function func_node(*tree_node); - std::cout << "Adding function: " << - parser::pddl::toString(tree, tree_node->node_id) << std::endl; + std::cout << "Adding function: " << parser::pddl::toString(tree, tree_node->node_id) + << std::endl; if (!addFunction(func_node)) { - std::cerr << "Failed to add function: " << parser::pddl::toString( - tree, - tree_node->node_id) << - std::endl; + std::cerr << "Failed to add function: " + << parser::pddl::toString(tree, tree_node->node_id) << std::endl; } - } - break; + } break; default: break; } diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertClient.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertClient.cpp index cecd3d488..3fd445eb1 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertClient.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertClient.cpp @@ -14,11 +14,11 @@ #include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include #include +#include +#include #include #include -#include #include "plansys2_pddl_parser/Utils.hpp" @@ -29,95 +29,128 @@ ProblemExpertClient::ProblemExpertClient() { node_ = rclcpp::Node::make_shared("problem_expert_client"); - add_problem_client_ = node_->create_client( - "problem_expert/add_problem"); - add_problem_goal_client_ = node_->create_client( - "problem_expert/add_problem_goal"); - add_problem_instance_client_ = node_->create_client( - "problem_expert/add_problem_instance"); - add_problem_predicate_client_ = node_->create_client( - "problem_expert/add_problem_predicate"); - add_problem_function_client_ = node_->create_client( - "problem_expert/add_problem_function"); - get_problem_goal_client_ = node_->create_client( - "problem_expert/get_problem_goal"); + add_problem_client_ = + node_->create_client("problem_expert/add_problem"); + add_problem_goal_client_ = + node_->create_client("problem_expert/add_problem_goal"); + add_problem_instance_client_ = + node_->create_client("problem_expert/add_problem_instance"); + add_problem_predicate_client_ = + node_->create_client("problem_expert/add_problem_predicate"); + add_problem_predicates_client_ = + node_->create_client("problem_expert/add_problem_predicates"); + update_problem_predicates_client_ = node_->create_client( + "problem_expert/update_problem_predicates"); + add_problem_function_client_ = + node_->create_client("problem_expert/add_problem_function"); + get_problem_goal_client_ = + node_->create_client("problem_expert/get_problem_goal"); + get_problem_state_client_ = + node_->create_client("problem_expert/get_problem_state"); get_problem_instance_details_client_ = node_->create_client( "problem_expert/get_problem_instance"); get_problem_instances_client_ = node_->create_client( "problem_expert/get_problem_instances"); - get_problem_predicate_details_client_ = - node_->create_client( + get_problem_predicate_details_client_ = node_->create_client( "problem_expert/get_problem_predicate"); - get_problem_predicates_client_ = node_->create_client( - "problem_expert/get_problem_predicates"); + get_problem_predicates_client_ = + node_->create_client("problem_expert/get_problem_predicates"); + get_problem_inferred_predicates_client_ = node_->create_client( + "problem_expert/get_problem_inferred_predicates"); get_problem_function_details_client_ = - node_->create_client( - "problem_expert/get_problem_function"); - get_problem_functions_client_ = node_->create_client( - "problem_expert/get_problem_functions"); - get_problem_client_ = node_->create_client( - "problem_expert/get_problem"); + node_->create_client("problem_expert/get_problem_function"); + get_problem_functions_client_ = + node_->create_client("problem_expert/get_problem_functions"); + get_problem_client_ = + node_->create_client("problem_expert/get_problem"); remove_problem_goal_client_ = node_->create_client( "problem_expert/remove_problem_goal"); clear_problem_knowledge_client_ = node_->create_client( "problem_expert/clear_problem_knowledge"); remove_problem_instance_client_ = - node_->create_client( - "problem_expert/remove_problem_instance"); + node_->create_client("problem_expert/remove_problem_instance"); remove_problem_predicate_client_ = - node_->create_client( - "problem_expert/remove_problem_predicate"); + node_->create_client("problem_expert/remove_problem_predicate"); + remove_problem_predicates_client_ = node_->create_client( + "problem_expert/remove_problem_predicates"); remove_problem_function_client_ = - node_->create_client( - "problem_expert/remove_problem_function"); + node_->create_client("problem_expert/remove_problem_function"); exist_problem_predicate_client_ = - node_->create_client( - "problem_expert/exist_problem_predicate"); + node_->create_client("problem_expert/exist_problem_predicate"); exist_problem_function_client_ = - node_->create_client( - "problem_expert/exist_problem_function"); + node_->create_client("problem_expert/exist_problem_function"); update_problem_function_client_ = - node_->create_client( - "problem_expert/update_problem_function"); + node_->create_client("problem_expert/update_problem_function"); is_problem_goal_satisfied_client_ = node_->create_client( "problem_expert/is_problem_goal_satisfied"); problem_sub_ = node_->create_subscription( - "problem_expert/problem", - rclcpp::QoS(100), [this](plansys2_msgs::msg::Problem::SharedPtr msg) { + "problem_expert/problem", rclcpp::QoS(100), [this](plansys2_msgs::msg::Problem::SharedPtr msg) { cached_problem_ = msg->problem; problem_ts_ = msg->stamp; }); update_problem_sub_ = node_->create_subscription( "problem_expert/update_notify", 100, - [this](std_msgs::msg::Empty::SharedPtr msg){ - update_time_ = this->node_->now(); - }); + [this](std_msgs::msg::Empty::SharedPtr msg) {update_time_ = this->node_->now();}); problem_ts_ = node_->now(); } -std::vector -ProblemExpertClient::getInstances() +plansys2::State ProblemExpertClient::getState() +{ + while (!get_problem_state_client_->wait_for_service(std::chrono::seconds(5))) { + if (!rclcpp::ok()) { + return {}; + } + RCLCPP_ERROR_STREAM( + node_->get_logger(), get_problem_state_client_->get_service_name() + << " service client: waiting for service to appear..."); + } + + auto request = std::make_shared(); + + auto future_result = get_problem_state_client_->async_send_request(request); + + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) + { + return {}; + } + + auto result = *future_result.get(); + + if (result.success) { + return plansys2::State(result.state); + } else { + RCLCPP_ERROR_STREAM( + node_->get_logger(), node_->get_namespace() + << get_problem_instances_client_->get_service_name() << ": " + << result.error_info); + return {}; + } +} + +std::unordered_set ProblemExpertClient::getInstances() { while (!get_problem_instances_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_instances_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_instances_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_problem_instances_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return {}; @@ -126,29 +159,26 @@ ProblemExpertClient::getInstances() auto result = *future_result.get(); if (result.success) { - return plansys2::convertVector( + return plansys2::convertVectorToUnorderedSet( result.instances); } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - node_->get_namespace() << - get_problem_instances_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), node_->get_namespace() + << get_problem_instances_client_->get_service_name() << ": " + << result.error_info); return {}; } } -bool -ProblemExpertClient::addInstance(const plansys2::Instance & instance) +bool ProblemExpertClient::addInstance(const plansys2::Instance & instance) { while (!add_problem_instance_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_instance_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), add_problem_instance_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -156,7 +186,8 @@ ProblemExpertClient::addInstance(const plansys2::Instance & instance) auto future_result = add_problem_instance_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -169,24 +200,21 @@ ProblemExpertClient::addInstance(const plansys2::Instance & instance) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_instance_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), add_problem_instance_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::removeInstance(const plansys2::Instance & instance) +bool ProblemExpertClient::removeInstance(const plansys2::Instance & instance) { while (!remove_problem_instance_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_instance_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), remove_problem_instance_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -194,7 +222,8 @@ ProblemExpertClient::removeInstance(const plansys2::Instance & instance) auto future_result = remove_problem_instance_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -207,25 +236,21 @@ ProblemExpertClient::removeInstance(const plansys2::Instance & instance) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_instance_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), remove_problem_instance_client_->get_service_name() + << ": " << result.error_info); return false; } } - -std::optional -ProblemExpertClient::getInstance(const std::string & name) +std::optional ProblemExpertClient::getInstance(const std::string & name) { while (!get_problem_instance_details_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_instance_details_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_instance_details_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -234,7 +259,8 @@ ProblemExpertClient::getInstance(const std::string & name) auto future_result = get_problem_instance_details_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return {}; @@ -246,31 +272,29 @@ ProblemExpertClient::getInstance(const std::string & name) return result.instance; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_instance_details_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_problem_instance_details_client_->get_service_name() + << ": " << result.error_info); return {}; } } -std::vector -ProblemExpertClient::getPredicates() +std::unordered_set ProblemExpertClient::getPredicates() { while (!get_problem_predicates_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_predicates_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_predicates_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_problem_predicates_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return {}; @@ -279,28 +303,60 @@ ProblemExpertClient::getPredicates() auto result = *future_result.get(); if (result.success) { - return plansys2::convertVector( + return plansys2::convertVectorToUnorderedSet( result.states); } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_predicates_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_problem_predicates_client_->get_service_name() + << ": " << result.error_info); return {}; } } -bool -ProblemExpertClient::addPredicate(const plansys2::Predicate & predicate) +std::unordered_set ProblemExpertClient::getInferredPredicates() +{ + while (!get_problem_inferred_predicates_client_->wait_for_service(std::chrono::seconds(5))) { + if (!rclcpp::ok()) { + return {}; + } + RCLCPP_ERROR_STREAM( + node_->get_logger(), get_problem_inferred_predicates_client_->get_service_name() + << " service client: waiting for service to appear..."); + } + + auto request = std::make_shared(); + + auto future_result = get_problem_inferred_predicates_client_->async_send_request(request); + + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) + { + return {}; + } + + auto result = *future_result.get(); + + if (result.success) { + return plansys2::convertVectorToUnorderedSet( + result.states); + } else { + RCLCPP_ERROR_STREAM( + node_->get_logger(), get_problem_inferred_predicates_client_->get_service_name() + << ": " << result.error_info); + return {}; + } +} + +bool ProblemExpertClient::addPredicate(const plansys2::Predicate & predicate) { while (!add_problem_predicate_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_predicate_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), add_problem_predicate_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -308,7 +364,44 @@ ProblemExpertClient::addPredicate(const plansys2::Predicate & predicate) auto future_result = add_problem_predicate_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + + auto result = *future_result.get(); + + if (result.success) { + update_time_ = node_->now(); + return true; + } else { + RCLCPP_ERROR_STREAM( + node_->get_logger(), add_problem_predicate_client_->get_service_name() + << ": " << result.error_info); + return false; + } +} + +bool ProblemExpertClient::addPredicates(const std::vector & predicates) +{ + while (!add_problem_predicates_client_->wait_for_service(std::chrono::seconds(5))) { + if (!rclcpp::ok()) { + return false; + } + RCLCPP_ERROR_STREAM( + node_->get_logger(), add_problem_predicates_client_->get_service_name() + << " service client: waiting for service to appear..."); + } + + auto request = std::make_shared(); + request->nodes = convertVector(predicates); + + auto future_result = add_problem_predicates_client_->async_send_request(request); + + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -321,24 +414,21 @@ ProblemExpertClient::addPredicate(const plansys2::Predicate & predicate) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_predicate_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), add_problem_predicates_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::removePredicate(const plansys2::Predicate & predicate) +bool ProblemExpertClient::removePredicate(const plansys2::Predicate & predicate) { while (!remove_problem_predicate_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_predicate_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), remove_problem_predicate_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -346,7 +436,8 @@ ProblemExpertClient::removePredicate(const plansys2::Predicate & predicate) auto future_result = remove_problem_predicate_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -359,24 +450,97 @@ ProblemExpertClient::removePredicate(const plansys2::Predicate & predicate) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_predicate_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), remove_problem_predicate_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::existPredicate(const plansys2::Predicate & predicate) +bool ProblemExpertClient::removePredicates(const std::vector & predicates) +{ + while (!remove_problem_predicates_client_->wait_for_service(std::chrono::seconds(5))) { + if (!rclcpp::ok()) { + return false; + } + RCLCPP_ERROR_STREAM( + node_->get_logger(), remove_problem_predicates_client_->get_service_name() + << " service client: waiting for service to appear..."); + } + + auto request = std::make_shared(); + request->nodes = convertVector(predicates); + + auto future_result = remove_problem_predicates_client_->async_send_request(request); + + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + + auto result = *future_result.get(); + + if (result.success) { + update_time_ = node_->now(); + return true; + } else { + RCLCPP_ERROR_STREAM( + node_->get_logger(), remove_problem_predicates_client_->get_service_name() + << ": " << result.error_info); + return false; + } +} + +bool ProblemExpertClient::updatePredicates( + const std::vector & add_predicates, + const std::vector & remove_predicates) +{ + while (!update_problem_predicates_client_->wait_for_service(std::chrono::seconds(5))) { + if (!rclcpp::ok()) { + return false; + } + RCLCPP_ERROR_STREAM( + node_->get_logger(), update_problem_predicates_client_->get_service_name() + << " service client: waiting for service to appear..."); + } + + auto request = std::make_shared(); + request->add_nodes = convertVector(add_predicates); + request->remove_nodes = + convertVector(remove_predicates); + + auto future_result = update_problem_predicates_client_->async_send_request(request); + + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + + auto result = *future_result.get(); + + if (result.success) { + update_time_ = node_->now(); + return true; + } else { + RCLCPP_ERROR_STREAM( + node_->get_logger(), update_problem_predicates_client_->get_service_name() + << ": " << result.error_info); + return false; + } +} + +bool ProblemExpertClient::existPredicate(const plansys2::Predicate & predicate) { while (!exist_problem_predicate_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - exist_problem_predicate_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), exist_problem_predicate_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -384,7 +548,8 @@ ProblemExpertClient::existPredicate(const plansys2::Predicate & predicate) auto future_result = exist_problem_predicate_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -395,17 +560,15 @@ ProblemExpertClient::existPredicate(const plansys2::Predicate & predicate) return result.exist; } -std::optional -ProblemExpertClient::getPredicate(const std::string & predicate) +std::optional ProblemExpertClient::getPredicate(const std::string & predicate) { while (!get_problem_predicate_details_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_predicate_details_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_predicate_details_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -414,7 +577,8 @@ ProblemExpertClient::getPredicate(const std::string & predicate) auto future_result = get_problem_predicate_details_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return {}; @@ -426,31 +590,29 @@ ProblemExpertClient::getPredicate(const std::string & predicate) return result.node; } else { RCLCPP_DEBUG_STREAM( - node_->get_logger(), - get_problem_predicate_details_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_problem_predicate_details_client_->get_service_name() + << ": " << result.error_info); return {}; } } -std::vector -ProblemExpertClient::getFunctions() +std::unordered_set ProblemExpertClient::getFunctions() { while (!get_problem_functions_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_functions_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_functions_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_problem_functions_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return {}; @@ -459,28 +621,25 @@ ProblemExpertClient::getFunctions() auto result = *future_result.get(); if (result.success) { - return plansys2::convertVector( + return plansys2::convertVectorToUnorderedSet( result.states); } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_functions_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_problem_functions_client_->get_service_name() + << ": " << result.error_info); return {}; } } -bool -ProblemExpertClient::addFunction(const plansys2::Function & function) +bool ProblemExpertClient::addFunction(const plansys2::Function & function) { while (!add_problem_function_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_function_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), add_problem_function_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -488,10 +647,8 @@ ProblemExpertClient::addFunction(const plansys2::Function & function) auto future_result = add_problem_function_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete( - node_, - future_result, - std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -504,24 +661,21 @@ ProblemExpertClient::addFunction(const plansys2::Function & function) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_function_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), add_problem_function_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::removeFunction(const plansys2::Function & function) +bool ProblemExpertClient::removeFunction(const plansys2::Function & function) { while (!remove_problem_function_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_function_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), remove_problem_function_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -529,7 +683,8 @@ ProblemExpertClient::removeFunction(const plansys2::Function & function) auto future_result = remove_problem_function_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -542,24 +697,21 @@ ProblemExpertClient::removeFunction(const plansys2::Function & function) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_function_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), remove_problem_function_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::existFunction(const plansys2::Function & function) +bool ProblemExpertClient::existFunction(const plansys2::Function & function) { while (!exist_problem_function_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - exist_problem_function_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), exist_problem_function_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -567,7 +719,8 @@ ProblemExpertClient::existFunction(const plansys2::Function & function) auto future_result = exist_problem_function_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -585,9 +738,8 @@ bool ProblemExpertClient::updateFunction(const plansys2::Function & function) return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - update_problem_function_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), update_problem_function_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -595,7 +747,8 @@ bool ProblemExpertClient::updateFunction(const plansys2::Function & function) auto future_result = update_problem_function_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -608,24 +761,21 @@ bool ProblemExpertClient::updateFunction(const plansys2::Function & function) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - update_problem_function_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), update_problem_function_client_->get_service_name() + << ": " << result.error_info); return false; } } -std::optional -ProblemExpertClient::getFunction(const std::string & function) +std::optional ProblemExpertClient::getFunction(const std::string & function) { while (!get_problem_function_details_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_function_details_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_function_details_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -634,7 +784,8 @@ ProblemExpertClient::getFunction(const std::string & function) auto future_result = get_problem_function_details_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return {}; @@ -646,16 +797,13 @@ ProblemExpertClient::getFunction(const std::string & function) return result.node; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_function_details_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_problem_function_details_client_->get_service_name() + << ": " << result.error_info); return {}; } } - -plansys2::Goal -ProblemExpertClient::getGoal() +plansys2::Goal ProblemExpertClient::getGoal() { plansys2_msgs::msg::Tree ret; @@ -664,16 +812,16 @@ ProblemExpertClient::getGoal() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_goal_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_goal_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_problem_goal_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return ret; @@ -685,25 +833,22 @@ ProblemExpertClient::getGoal() return result.tree; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_goal_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_problem_goal_client_->get_service_name() + << ": " << result.error_info); } return ret; } -bool -ProblemExpertClient::setGoal(const plansys2::Goal & goal) +bool ProblemExpertClient::setGoal(const plansys2::Goal & goal) { while (!add_problem_goal_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_goal_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), add_problem_goal_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -711,7 +856,8 @@ ProblemExpertClient::setGoal(const plansys2::Goal & goal) auto future_result = add_problem_goal_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -724,24 +870,21 @@ ProblemExpertClient::setGoal(const plansys2::Goal & goal) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_goal_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), add_problem_goal_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::isGoalSatisfied(const plansys2::Goal & goal) +bool ProblemExpertClient::isGoalSatisfied(const plansys2::Goal & goal) { while (!is_problem_goal_satisfied_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - is_problem_goal_satisfied_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), is_problem_goal_satisfied_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -749,7 +892,8 @@ ProblemExpertClient::isGoalSatisfied(const plansys2::Goal & goal) auto future_result = is_problem_goal_satisfied_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -761,31 +905,29 @@ ProblemExpertClient::isGoalSatisfied(const plansys2::Goal & goal) return result.satisfied; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - is_problem_goal_satisfied_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), is_problem_goal_satisfied_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::clearGoal() +bool ProblemExpertClient::clearGoal() { while (!remove_problem_goal_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_goal_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), remove_problem_goal_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = remove_problem_goal_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -798,31 +940,29 @@ ProblemExpertClient::clearGoal() return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - remove_problem_goal_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), remove_problem_goal_client_->get_service_name() + << ": " << result.error_info); return false; } } -bool -ProblemExpertClient::clearKnowledge() +bool ProblemExpertClient::clearKnowledge() { while (!clear_problem_knowledge_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - clear_problem_knowledge_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), clear_problem_knowledge_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = clear_problem_knowledge_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -835,21 +975,18 @@ ProblemExpertClient::clearKnowledge() return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - clear_problem_knowledge_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), clear_problem_knowledge_client_->get_service_name() + << ": " << result.error_info); return false; } } -std::tuple -ProblemExpertClient::getProblemWithTimestamp(bool use_cache) +std::tuple ProblemExpertClient::getProblemWithTimestamp(bool use_cache) { return {getProblem(use_cache), problem_ts_}; } -std::string -ProblemExpertClient::getProblem(bool use_cache) +std::string ProblemExpertClient::getProblem(bool use_cache) { if (use_cache && cached_problem_ != "") { return cached_problem_; @@ -858,24 +995,23 @@ ProblemExpertClient::getProblem(bool use_cache) } } -std::string -ProblemExpertClient::getProblem() +std::string ProblemExpertClient::getProblem() { while (!get_problem_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_problem_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_problem_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return {}; @@ -890,24 +1026,20 @@ ProblemExpertClient::getProblem() return result.problem; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_problem_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_problem_client_->get_service_name() << ": " << result.error_info); return {}; } } -bool -ProblemExpertClient::addProblem(const std::string & problem_str) +bool ProblemExpertClient::addProblem(const std::string & problem_str) { while (!add_problem_client_->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { return false; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), add_problem_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -915,7 +1047,8 @@ ProblemExpertClient::addProblem(const std::string & problem_str) auto future_result = add_problem_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -928,9 +1061,7 @@ ProblemExpertClient::addProblem(const std::string & problem_str) return true; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - add_problem_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), add_problem_client_->get_service_name() << ": " << result.error_info); return false; } } diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp index a7704d6f9..ab5f8fe28 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp @@ -14,8 +14,8 @@ #include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include #include +#include #include #include "plansys2_pddl_parser/Utils.hpp" @@ -51,175 +51,179 @@ ProblemExpertNode::ProblemExpertNode() add_problem_service_ = create_service( "problem_expert/add_problem", std::bind( - &ProblemExpertNode::add_problem_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::add_problem_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); add_problem_goal_service_ = create_service( "problem_expert/add_problem_goal", std::bind( - &ProblemExpertNode::add_problem_goal_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::add_problem_goal_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); add_problem_instance_service_ = create_service( "problem_expert/add_problem_instance", std::bind( - &ProblemExpertNode::add_problem_instance_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::add_problem_instance_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); add_problem_predicate_service_ = create_service( "problem_expert/add_problem_predicate", std::bind( - &ProblemExpertNode::add_problem_predicate_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::add_problem_predicate_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + add_problem_predicates_service_ = create_service( + "problem_expert/add_problem_predicates", + std::bind( + &ProblemExpertNode::add_problem_predicates_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + update_problem_predicates_service_ = create_service( + "problem_expert/update_problem_predicates", + std::bind( + &ProblemExpertNode::update_problem_predicates_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); add_problem_function_service_ = create_service( "problem_expert/add_problem_function", std::bind( - &ProblemExpertNode::add_problem_function_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::add_problem_function_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_problem_goal_service_ = create_service( "problem_expert/get_problem_goal", std::bind( - &ProblemExpertNode::get_problem_goal_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::get_problem_goal_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + get_problem_state_service_ = create_service( + "problem_expert/get_problem_state", + std::bind( + &ProblemExpertNode::get_problem_state_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_problem_instance_details_service_ = create_service( "problem_expert/get_problem_instance", std::bind( - &ProblemExpertNode::get_problem_instance_details_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::get_problem_instance_details_service_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); get_problem_instances_service_ = create_service( "problem_expert/get_problem_instances", std::bind( - &ProblemExpertNode::get_problem_instances_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::get_problem_instances_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); - get_problem_predicate_details_service_ = - create_service( - "problem_expert/get_problem_predicate", std::bind( - &ProblemExpertNode::get_problem_predicate_details_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + get_problem_predicate_details_service_ = create_service( + "problem_expert/get_problem_predicate", + std::bind( + &ProblemExpertNode::get_problem_predicate_details_service_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); get_problem_predicates_service_ = create_service( "problem_expert/get_problem_predicates", std::bind( - &ProblemExpertNode::get_problem_predicates_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::get_problem_predicates_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + get_problem_inferred_predicates_service_ = create_service( + "problem_expert/get_problem_inferred_predicates", + std::bind( + &ProblemExpertNode::get_problem_inferred_predicates_service_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - get_problem_function_details_service_ = - create_service( - "problem_expert/get_problem_function", std::bind( - &ProblemExpertNode::get_problem_function_details_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + get_problem_function_details_service_ = create_service( + "problem_expert/get_problem_function", + std::bind( + &ProblemExpertNode::get_problem_function_details_service_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); get_problem_functions_service_ = create_service( "problem_expert/get_problem_functions", std::bind( - &ProblemExpertNode::get_problem_functions_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::get_problem_functions_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_problem_service_ = create_service( - "problem_expert/get_problem", std::bind( - &ProblemExpertNode::get_problem_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "problem_expert/get_problem", + std::bind( + &ProblemExpertNode::get_problem_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); is_problem_goal_satisfied_service_ = create_service( - "problem_expert/is_problem_goal_satisfied", std::bind( - &ProblemExpertNode::is_problem_goal_satisfied_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "problem_expert/is_problem_goal_satisfied", + std::bind( + &ProblemExpertNode::is_problem_goal_satisfied_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); remove_problem_goal_service_ = create_service( "problem_expert/remove_problem_goal", std::bind( - &ProblemExpertNode::remove_problem_goal_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::remove_problem_goal_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); clear_problem_knowledge_service_ = create_service( "problem_expert/clear_problem_knowledge", std::bind( - &ProblemExpertNode::clear_problem_knowledge_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::clear_problem_knowledge_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); remove_problem_instance_service_ = create_service( "problem_expert/remove_problem_instance", std::bind( - &ProblemExpertNode::remove_problem_instance_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::remove_problem_instance_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); remove_problem_predicate_service_ = create_service( "problem_expert/remove_problem_predicate", std::bind( - &ProblemExpertNode::remove_problem_predicate_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::remove_problem_predicate_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + remove_problem_predicates_service_ = create_service( + "problem_expert/remove_problem_predicates", + std::bind( + &ProblemExpertNode::remove_problem_predicates_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); remove_problem_function_service_ = create_service( "problem_expert/remove_problem_function", std::bind( - &ProblemExpertNode::remove_problem_function_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::remove_problem_function_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); exist_problem_predicate_service_ = create_service( "problem_expert/exist_problem_predicate", std::bind( - &ProblemExpertNode::exist_problem_predicate_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::exist_problem_predicate_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); exist_problem_function_service_ = create_service( "problem_expert/exist_problem_function", std::bind( - &ProblemExpertNode::exist_problem_function_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::exist_problem_function_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); update_problem_function_service_ = create_service( "problem_expert/update_problem_function", std::bind( - &ProblemExpertNode::update_problem_function_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ProblemExpertNode::update_problem_function_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); - problem_pub_ = create_publisher( - "problem_expert/problem", - rclcpp::QoS(100)); + problem_pub_ = + create_publisher("problem_expert/problem", rclcpp::QoS(100)); - update_pub_ = create_publisher( - "problem_expert/update_notify", - rclcpp::QoS(100)); + update_pub_ = + create_publisher("problem_expert/update_notify", rclcpp::QoS(100)); knowledge_pub_ = create_publisher( - "problem_expert/knowledge", - rclcpp::QoS(100).transient_local()); + "problem_expert/knowledge", rclcpp::QoS(100).transient_local()); } +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -CallbackReturnT -ProblemExpertNode::on_configure(const rclcpp_lifecycle::State & state) +CallbackReturnT ProblemExpertNode::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Configuring...", get_name()); @@ -230,17 +234,15 @@ ProblemExpertNode::on_configure(const rclcpp_lifecycle::State & state) auto model_files = tokenize(model_file, ":"); std::ifstream domain_first_ifs(model_files[0]); - std::string domain_first_str(( - std::istreambuf_iterator(domain_first_ifs)), - std::istreambuf_iterator()); + std::string domain_first_str( + (std::istreambuf_iterator(domain_first_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_first_str); for (size_t i = 1; i < model_files.size(); i++) { std::ifstream domain_ifs(model_files[i]); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); domain_expert->extendDomain(domain_str); } @@ -249,9 +251,8 @@ ProblemExpertNode::on_configure(const rclcpp_lifecycle::State & state) auto problem_file = get_parameter("problem_file").get_value(); if (!problem_file.empty()) { std::ifstream problem_ifs(problem_file); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); problem_expert_->addProblem(problem_str); } @@ -259,8 +260,7 @@ ProblemExpertNode::on_configure(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ProblemExpertNode::on_activate(const rclcpp_lifecycle::State & state) +CallbackReturnT ProblemExpertNode::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Activating...", get_name()); update_pub_->on_activate(); @@ -270,8 +270,7 @@ ProblemExpertNode::on_activate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ProblemExpertNode::on_deactivate(const rclcpp_lifecycle::State & state) +CallbackReturnT ProblemExpertNode::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Deactivating...", get_name()); update_pub_->on_deactivate(); @@ -282,8 +281,7 @@ ProblemExpertNode::on_deactivate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ProblemExpertNode::on_cleanup(const rclcpp_lifecycle::State & state) +CallbackReturnT ProblemExpertNode::on_cleanup(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Cleaning up...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Cleaned up", get_name()); @@ -291,8 +289,7 @@ ProblemExpertNode::on_cleanup(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ProblemExpertNode::on_shutdown(const rclcpp_lifecycle::State & state) +CallbackReturnT ProblemExpertNode::on_shutdown(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Shutting down...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Shutted down", get_name()); @@ -300,16 +297,14 @@ ProblemExpertNode::on_shutdown(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ProblemExpertNode::on_error(const rclcpp_lifecycle::State & state) +CallbackReturnT ProblemExpertNode::on_error(const rclcpp_lifecycle::State & state) { RCLCPP_ERROR(get_logger(), "[%s] Error transition", get_name()); return CallbackReturnT::SUCCESS; } -void -ProblemExpertNode::add_problem_service_callback( +void ProblemExpertNode::add_problem_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -336,8 +331,7 @@ ProblemExpertNode::add_problem_service_callback( } } -void -ProblemExpertNode::add_problem_goal_service_callback( +void ProblemExpertNode::add_problem_goal_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -367,8 +361,7 @@ ProblemExpertNode::add_problem_goal_service_callback( } } -void -ProblemExpertNode::add_problem_instance_service_callback( +void ProblemExpertNode::add_problem_instance_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -393,8 +386,7 @@ ProblemExpertNode::add_problem_instance_service_callback( } } -void -ProblemExpertNode::add_problem_predicate_service_callback( +void ProblemExpertNode::add_problem_predicate_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -414,14 +406,56 @@ ProblemExpertNode::add_problem_predicate_service_callback( problem_msg.stamp = now(); problem_pub_->publish(problem_msg); } else { - response->error_info = - "Predicate [" + parser::pddl::toString(request->node) + "] not valid"; + response->error_info = "Predicate [" + parser::pddl::toString(request->node) + "] not valid"; + } + } +} + +void ProblemExpertNode::add_problem_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + if (problem_expert_ == nullptr) { + response->success = false; + response->error_info = "Requesting service in non-active state"; + RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); + } else { + response->success = problem_expert_->addPredicates( + convertVector(request->nodes)); + if (response->success) { + update_pub_->publish(std_msgs::msg::Empty()); + knowledge_pub_->publish(*get_knowledge_as_msg()); + } else { + response->error_info = "One of the predicates is not valid"; + } + } +} + +void ProblemExpertNode::update_problem_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + response->success = true; + if (problem_expert_ == nullptr) { + response->success = false; + response->error_info = "Requesting service in non-active state"; + RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); + } else { + response->success &= problem_expert_->updatePredicates( + convertVector(request->add_nodes), + convertVector(request->remove_nodes)); + if (response->success) { + update_pub_->publish(std_msgs::msg::Empty()); + knowledge_pub_->publish(*get_knowledge_as_msg()); + } else { + response->error_info = "One of the predicates is not valid"; } } } -void -ProblemExpertNode::add_problem_function_service_callback( +void ProblemExpertNode::add_problem_function_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -441,14 +475,12 @@ ProblemExpertNode::add_problem_function_service_callback( problem_msg.stamp = now(); problem_pub_->publish(problem_msg); } else { - response->error_info = - "Function [" + parser::pddl::toString(request->node) + "] not valid"; + response->error_info = "Function [" + parser::pddl::toString(request->node) + "] not valid"; } } } -void -ProblemExpertNode::get_problem_goal_service_callback( +void ProblemExpertNode::get_problem_goal_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -463,8 +495,22 @@ ProblemExpertNode::get_problem_goal_service_callback( } } -void -ProblemExpertNode::get_problem_instance_details_service_callback( +void ProblemExpertNode::get_problem_state_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + if (problem_expert_ == nullptr) { + response->success = false; + response->error_info = "Requesting service in non-active state"; + RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); + } else { + response->success = true; + response->state = problem_expert_->getState().getAsMsg(); + } +} + +void ProblemExpertNode::get_problem_instance_details_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -485,8 +531,7 @@ ProblemExpertNode::get_problem_instance_details_service_callback( } } -void -ProblemExpertNode::get_problem_instances_service_callback( +void ProblemExpertNode::get_problem_instances_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -497,13 +542,13 @@ ProblemExpertNode::get_problem_instances_service_callback( RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); } else { response->success = true; - response->instances = plansys2::convertVector( + response->instances = + plansys2::convertUnorderedSetToVector( problem_expert_->getInstances()); } } -void -ProblemExpertNode::get_problem_predicate_details_service_callback( +void ProblemExpertNode::get_problem_predicate_details_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -524,8 +569,7 @@ ProblemExpertNode::get_problem_predicate_details_service_callback( } } -void -ProblemExpertNode::get_problem_predicates_service_callback( +void ProblemExpertNode::get_problem_predicates_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -536,13 +580,30 @@ ProblemExpertNode::get_problem_predicates_service_callback( RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); } else { response->success = true; - response->states = plansys2::convertVector( + response->states = + plansys2::convertUnorderedSetToVector( problem_expert_->getPredicates()); } } -void -ProblemExpertNode::get_problem_function_details_service_callback( +void ProblemExpertNode::get_problem_inferred_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + if (problem_expert_ == nullptr) { + response->success = false; + response->error_info = "Requesting service in non-active state"; + RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); + } else { + response->success = true; + response->states = + plansys2::convertUnorderedSetToVector( + problem_expert_->getInferredPredicates()); + } +} + +void ProblemExpertNode::get_problem_function_details_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -563,8 +624,7 @@ ProblemExpertNode::get_problem_function_details_service_callback( } } -void -ProblemExpertNode::get_problem_functions_service_callback( +void ProblemExpertNode::get_problem_functions_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -575,13 +635,13 @@ ProblemExpertNode::get_problem_functions_service_callback( RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); } else { response->success = true; - response->states = plansys2::convertVector( + response->states = + plansys2::convertUnorderedSetToVector( problem_expert_->getFunctions()); } } -void -ProblemExpertNode::get_problem_service_callback( +void ProblemExpertNode::get_problem_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -597,8 +657,7 @@ ProblemExpertNode::get_problem_service_callback( } } -void -ProblemExpertNode::is_problem_goal_satisfied_service_callback( +void ProblemExpertNode::is_problem_goal_satisfied_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -613,8 +672,7 @@ ProblemExpertNode::is_problem_goal_satisfied_service_callback( } } -void -ProblemExpertNode::remove_problem_goal_service_callback( +void ProblemExpertNode::remove_problem_goal_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -640,8 +698,7 @@ ProblemExpertNode::remove_problem_goal_service_callback( } } -void -ProblemExpertNode::clear_problem_knowledge_service_callback( +void ProblemExpertNode::clear_problem_knowledge_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -667,9 +724,7 @@ ProblemExpertNode::clear_problem_knowledge_service_callback( } } - -void -ProblemExpertNode::remove_problem_instance_service_callback( +void ProblemExpertNode::remove_problem_instance_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -695,8 +750,7 @@ ProblemExpertNode::remove_problem_instance_service_callback( } } -void -ProblemExpertNode::remove_problem_predicate_service_callback( +void ProblemExpertNode::remove_problem_predicate_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -721,8 +775,28 @@ ProblemExpertNode::remove_problem_predicate_service_callback( } } -void -ProblemExpertNode::remove_problem_function_service_callback( +void ProblemExpertNode::remove_problem_predicates_service_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + if (problem_expert_ == nullptr) { + response->success = false; + response->error_info = "Requesting service in non-active state"; + RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); + } else { + response->success = problem_expert_->removePredicates( + convertVector(request->nodes)); + if (response->success) { + update_pub_->publish(std_msgs::msg::Empty()); + knowledge_pub_->publish(*get_knowledge_as_msg()); + } else { + response->error_info = "Error removing predicate"; + } + } +} + +void ProblemExpertNode::remove_problem_function_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -741,8 +815,7 @@ ProblemExpertNode::remove_problem_function_service_callback( } } -void -ProblemExpertNode::exist_problem_predicate_service_callback( +void ProblemExpertNode::exist_problem_predicate_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -755,8 +828,7 @@ ProblemExpertNode::exist_problem_predicate_service_callback( } } -void -ProblemExpertNode::exist_problem_function_service_callback( +void ProblemExpertNode::exist_problem_function_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -769,8 +841,7 @@ ProblemExpertNode::exist_problem_function_service_callback( } } -void -ProblemExpertNode::update_problem_function_service_callback( +void ProblemExpertNode::update_problem_function_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -789,8 +860,7 @@ ProblemExpertNode::update_problem_function_service_callback( } } -plansys2_msgs::msg::Knowledge::SharedPtr -ProblemExpertNode::get_knowledge_as_msg() const +plansys2_msgs::msg::Knowledge::SharedPtr ProblemExpertNode::get_knowledge_as_msg() const { auto ret_msgs = std::make_shared(); diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp index 287f98e83..3f72f65b1 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp @@ -12,272 +12,718 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_problem_expert/Utils.hpp" + +#include // OpenMP for parallelization + +#include #include -#include -#include #include -#include +#include +#include +#include #include +#include -#include "plansys2_problem_expert/Utils.hpp" #include "plansys2_pddl_parser/Utils.hpp" namespace plansys2 { -std::tuple evaluate( - const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - std::vector & predicates, - std::vector & functions, - bool apply, - bool use_state, - uint8_t node_id, +std::tuple>> unifyPredicate( + const plansys2::Predicate & predicate, const std::unordered_set & predicates) +{ + std::vector> param_dict_vector; + const size_t param_count = predicate.parameters.size(); + std::map variable_parameters; + for (size_t i = 0; i < param_count; ++i) { + // If the parameter name starts with '?', store the mapping + if (predicate.parameters[i].name.front() == '?') { + variable_parameters[predicate.parameters[i].name] = i; + } + } + + if (variable_parameters.empty()) { + return std::make_tuple(predicates.find(predicate) != predicates.end(), param_dict_vector); + } + + param_dict_vector.reserve(predicates.size()); + bool result = false; + + for (const auto & p : predicates) { + if (parser::pddl::checkNodeEquality(p, predicate, false)) { + std::map params_dict; + + for (const auto & variable : variable_parameters) { + params_dict.emplace(variable.first, p.parameters[variable.second].name); + } + result = true; + if (!params_dict.empty()) { + param_dict_vector.emplace_back(std::move(params_dict)); + } + } + } + + return std::make_tuple(result, std::move(param_dict_vector)); +} + +std::tuple>> unifyFunction( + const plansys2::Function & function, const std::unordered_set & functions) +{ + std::vector> param_dict_vector; + param_dict_vector.reserve(functions.size()); + + bool result = false; + const size_t param_count = function.parameters.size(); + + for (const plansys2::Function & p : functions) { + if (parser::pddl::checkNodeEquality(p, function, false)) { + std::map params_dict; + + for (size_t i = 0; i < param_count; ++i) { + // If the parameter name starts with '?', store the mapping + if (function.parameters[i].name.front() == '?') { + params_dict.emplace(function.parameters[i].name, p.parameters[i].name); + } + } + result = true; + if (params_dict.empty()) { + return std::make_tuple(result, std::move(param_dict_vector)); + } + param_dict_vector.emplace_back(std::move(params_dict)); + } + } + + return std::make_tuple(result, std::move(param_dict_vector)); +} + +std::vector> complementParamsValuesVector( + const std::vector & params, + const std::vector> & param_dict_vector, + const std::unordered_set & instances) +{ + std::vector> parameters_vector; + parameters_vector.reserve(params.size()); + + for (size_t i = 0; i < params.size(); i++) { + std::vector p_vector; + for (const auto & instance : instances) { + if (parser::pddl::checkParamTypeEquivalence(params[i], instance)) { + p_vector.emplace_back(instance.name); + } + } + parameters_vector.emplace_back(std::move(p_vector)); + } + + std::vector> complement_set; + if (parameters_vector.empty()) { + return complement_set; + } + + complement_set.emplace_back(); + + for (size_t i = 0; i < parameters_vector.size(); i++) { + std::vector> temp_result; + temp_result.reserve(complement_set.size() * parameters_vector[i].size()); + + for (const auto & combination : complement_set) { + for (const auto & element : parameters_vector[i]) { + std::map new_combination = combination; + new_combination[params[i].name] = element; + + if ( + i == parameters_vector.size() - 1 && + std::find(param_dict_vector.begin(), param_dict_vector.end(), new_combination) != + param_dict_vector.end()) + { + continue; + } + temp_result.emplace_back(new_combination); + } + } + complement_set = std::move(temp_result); + } + return std::move(complement_set); +} + +std::tuple>> negateResult( + const plansys2_msgs::msg::Node & node, const bool & result, + const std::vector> & param_dict_vector, + const std::unordered_set & instances) +{ + std::vector params = get_node_free_parameters(node); + return negateResult(params, result, param_dict_vector, instances); +} + +std::tuple>> negateResult( + const std::vector & params, const bool & result, + const std::vector> & param_dict_vector, + const std::unordered_set & instances) +{ + if (params.empty()) { + return {static_cast(true ^ result), {}}; + } + + auto complement_param_dict_vector = + complementParamsValuesVector(params, param_dict_vector, instances); + return { + !result || (result && !complement_param_dict_vector.empty()), + std::move(complement_param_dict_vector)}; +} + +std::vector get_node_free_parameters( + const plansys2_msgs::msg::Node & node) +{ + std::vector params; + std::unordered_set seen; + get_node_free_parameters_impl(node, params, seen); + return params; +} + +void get_node_free_parameters_impl( + const plansys2_msgs::msg::Node & node, std::vector & params, + std::unordered_set & seen) +{ + // std::vector params; + // std::unordered_set seen; + + for (const auto & param : node.parameters) { + if (!param.name.empty() && param.name.front() == '?') { + if (seen.insert(param.name).second) { + params.push_back(param); + } + } + } + // return params; +} + +std::vector get_node_children_free_parameters( + const plansys2_msgs::msg::Tree & tree, const plansys2_msgs::msg::Node & current_node) +{ + std::vector params; + std::unordered_set seen; + std::unordered_set exists_params; + get_node_children_free_parameters_impl(tree, current_node, params, seen, exists_params); + return params; +} + +void get_node_children_free_parameters_impl( + const plansys2_msgs::msg::Tree & tree, const plansys2_msgs::msg::Node & current_node, + std::vector & params, std::unordered_set & seen, + std::unordered_set & exists_params) +{ + auto current_node_params = get_node_free_parameters(current_node); + if (current_node.node_type == plansys2_msgs::msg::Node::EXISTS) { + for (const auto & param : current_node_params) { + exists_params.insert(param.name); + } + } + + if (current_node.node_type != plansys2_msgs::msg::Node::EXISTS) { + for (const auto & param : current_node_params) { + if (seen.insert(param.name).second && exists_params.find(param.name) == exists_params.end()) { + params.push_back(param); + } + } + } + + for (const auto & child_id : current_node.children) { + get_node_children_free_parameters_impl(tree, tree.nodes[child_id], params, seen, exists_params); + } +} + +void mergeParamsValuesDicts( + const std::map & dict1, + const std::map & dict2, std::map & dict3) +{ + dict3.clear(); + + auto it1 = dict1.begin(); + auto it2 = dict2.begin(); + + // Iterate through both maps simultaneously + while (it1 != dict1.end() && it2 != dict2.end()) { + if (it1->first < it2->first) { + dict3.emplace(it1->first, it1->second); // Insert from dict1 + ++it1; + } else if (it1->first > it2->first) { + dict3.emplace(it2->first, it2->second); // Insert from dict2 + ++it2; + } else { + // Keys are equal, check if values are the same + if (it1->second != it2->second) { + dict3.clear(); + return; // Different values for same parameter, return empty dict + } + dict3.emplace(it1->first, it1->second); // Insert the common element + ++it1; + ++it2; + } + } + + // Insert remaining elements from dict1 + while (it1 != dict1.end()) { + dict3.emplace(it1->first, it1->second); + ++it1; + } + + // Insert remaining elements from dict2 + while (it2 != dict2.end()) { + dict3.emplace(it2->first, it2->second); + ++it2; + } +} + +std::vector> mergeParamsValuesVector( + const std::vector> & vector1, + const std::vector> & vector2) +{ + std::vector>> thread_locals; + std::vector> thread_signatures; + + int n_threads = omp_get_max_threads(); + thread_locals.resize(n_threads); + thread_signatures.resize(n_threads); + + auto map_to_string = [](const std::map & m) { + std::ostringstream oss; + for (const auto & [k, v] : m) { + oss << k << '=' << v << ';'; + } + return oss.str(); + }; + +#pragma omp parallel + { + int tid = omp_get_thread_num(); + auto & local_vec = thread_locals[tid]; + auto & local_sig = thread_signatures[tid]; + +#pragma omp for schedule(dynamic) nowait + for (size_t i = 0; i < vector1.size(); ++i) { + for (const auto & dict2 : vector2) { + std::map dict3; + mergeParamsValuesDicts(vector1[i], dict2, dict3); + if (!dict3.empty()) { + std::string sig = map_to_string(dict3); + if (local_sig.insert(sig).second) { + local_vec.emplace_back(std::move(dict3)); + } + } + } + } + } + + // Merge thread-local results, keeping global uniqueness and vector output + std::vector> result; + std::unordered_set global_signatures; + for (const auto & local_vec : thread_locals) { + for (const auto & dict : local_vec) { + std::string sig = map_to_string(dict); + if (global_signatures.insert(sig).second) { + result.push_back(dict); // Keep order of first occurrence + } + } + } + return result; +} + +void solveDerivedPredicates(plansys2::State & state) +{ + std::vector root_nodes; + solveDerivedPredicates(state, root_nodes); +} + +void solveDerivedPredicates( + plansys2::State & state, const std::vector & root_nodes) +{ + if (root_nodes.empty()) { + state.resetInferredPredicates(); + } + + std::unordered_set derived_ungrounded_cache; + auto sccs = state.getDerivedPredicatesSCCs(root_nodes); + + for (const auto & scc : sccs) { + if (scc.size() == 1) { // Acyclic SCC + evaluateSCC(scc, state, root_nodes, derived_ungrounded_cache); + } else { // Cyclic SCC + std::unordered_set fixpoint_cache; + bool changed = true; + while (changed) { + changed = evaluateSCC(scc, state, root_nodes, fixpoint_cache); + } + } + } +} + +bool evaluateSCC( + const std::vector & scc, plansys2::State & state, + const std::vector & root_nodes, + std::unordered_set & unground_cache) +{ + bool changed_flag = false; + for (const auto & derived : scc) { + if (!root_nodes.empty() && unground_cache.find(derived) == unground_cache.end()) { + auto derived_removed = state.ungroundDerivedPredicate(derived); + unground_cache.insert(derived); + unground_cache.insert(derived_removed.begin(), derived_removed.end()); + } + size_t inferred_size_before = state.getInferredPredicatesSize(); + auto [_, evaluate_value, __, params_values] = + evaluate(derived.preconditions, state, derived.preconditions.nodes[0].node_id); + + if (evaluate_value && !params_values.empty()) { + groundPredicate(state, derived, params_values); + changed_flag |= (inferred_size_before != state.getInferredPredicatesSize()); + } + } + return changed_flag; +} + +void groundPredicate( + plansys2::State & state, const plansys2::Derived & derived, + const std::vector> & params_values_vector) +{ + size_t num_params = derived.predicate.parameters.size(); + size_t params_values_size = params_values_vector.size(); + + state.reserveInferredPredicates(state.getUnionPredicatesSize() + params_values_size); + auto instances = state.getInstances(); + + // Add this before the parallel region + std::vector thread_times(omp_get_max_threads(), 0.0); + + size_t n_threads = omp_get_max_threads(); + std::vector> thread_local_pred_sets(n_threads); + for (auto & v : thread_local_pred_sets) { + v.reserve(params_values_size / n_threads); + } + + std::vector param_keys(num_params); + for (size_t i = 0; i < num_params; ++i) { + param_keys[i] = "?" + std::to_string(i); + } + +#pragma omp parallel for schedule(dynamic) + for (size_t j = 0; j < params_values_size; ++j) { + const auto & params_values = params_values_vector[j]; + plansys2::Predicate new_predicate; + new_predicate.node_type = plansys2_msgs::msg::Node::PREDICATE; + new_predicate.name = derived.predicate.name; + new_predicate.parameters.reserve(num_params); + bool add_predicate = true; + + for (size_t i = 0; i < num_params; ++i) { + plansys2_msgs::msg::Param new_param = derived.predicate.parameters[i]; + + // Only perform lookup and assignment if the parameter is a variable (starts with '?') + if (new_param.name.front() == '?') { + auto it = params_values.find(param_keys[i]); + if (it != params_values.end()) { + auto instance = instances.find(parser::pddl::fromStringParam(it->second)); + if ( + instance == instances.end() || + !parser::pddl::checkParamTypeEquivalence(new_param, *instance)) + { + add_predicate = false; + break; + } + new_param.name = it->second; + } + } + new_predicate.parameters.emplace_back(std::move(new_param)); + } + + if (add_predicate) { + thread_local_pred_sets[omp_get_thread_num()].emplace(std::move(new_predicate)); + } + } + + for (auto & pred_vec : thread_local_pred_sets) { + for (auto & pred : pred_vec) { + state.addInferredPredicate(derived, std::move(pred)); + } + } +} + +std::tuple>> evaluate( + const plansys2_msgs::msg::Tree & tree, const plansys2::State & state, uint8_t node_id, bool negate) { - if (tree.nodes.empty()) { // No expression - return std::make_tuple(true, true, 0); + if (tree.nodes.empty()) { + return {true, true, 0, {}}; } - switch (tree.nodes[node_id].node_type) { + const auto & current_node = tree.nodes[node_id]; + switch (current_node.node_type) { case plansys2_msgs::msg::Node::AND: { bool success = true; bool truth_value = true; + std::vector> param_values; + std::vector child_nodes_free_params; + std::unordered_set existing_child_nodes_free_params; + + for (const auto & child_id : current_node.children) { + auto [child_success, child_value, _, child_param_values] = + evaluate(tree, state, child_id, false); + + success &= child_success; + truth_value &= child_value; + if (!truth_value) { + return {success, static_cast(negate ^ truth_value), 0, {}}; + break; + } - for (auto & child_id : tree.nodes[node_id].children) { - std::tuple result = - evaluate( - tree, problem_client, predicates, functions, apply, use_state, child_id, - negate); - success = success && std::get<0>(result); - truth_value = truth_value && std::get<1>(result); + if (negate) { + auto child_node = tree.nodes[child_id]; + auto child_free_params = get_node_free_parameters(child_node); + for (const auto & p : child_free_params) { + if (existing_child_nodes_free_params.insert(p.name) + .second) // Only insert if not already present + { + child_nodes_free_params.push_back(p); + } + } + } + + if (param_values.empty()) { + param_values = std::move(child_param_values); + } else if (!child_param_values.empty()) { + param_values = mergeParamsValuesVector(param_values, std::move(child_param_values)); + if (param_values.empty()) { + return {success, negate, 0, {}}; + } + } } - return std::make_tuple(success, truth_value, 0); + if (negate) { + std::tie(truth_value, param_values) = + negateResult(child_nodes_free_params, truth_value, param_values, state.getInstances()); + } + return {success, truth_value, 0, std::move(param_values)}; } case plansys2_msgs::msg::Node::OR: { bool success = true; bool truth_value = false; - - for (auto & child_id : tree.nodes[node_id].children) { - std::tuple result = - evaluate( - tree, problem_client, predicates, functions, apply, use_state, child_id, - negate); - success = success && std::get<0>(result); - truth_value = truth_value || std::get<1>(result); + std::vector> param_values; + + std::vector child_nodes_free_params; + std::unordered_set existing_child_nodes_free_params; + + for (auto & child_id : current_node.children) { + auto [child_success, child_value, _, child_param_values] = + evaluate(tree, state, child_id, false); + + success = success && child_success; + truth_value = truth_value || child_value; + param_values.insert( + param_values.end(), child_param_values.begin(), child_param_values.end()); + + if (negate) { + auto child_node = tree.nodes[child_id]; + auto child_free_params = get_node_free_parameters(child_node); + for (const auto & p : child_free_params) { + if (existing_child_nodes_free_params.insert(p.name) + .second) // Only insert if not already present + { + child_nodes_free_params.push_back(p); + } + } + } } - return std::make_tuple(success, truth_value, 0); + if (negate) { + std::tie(truth_value, param_values) = + negateResult(child_nodes_free_params, truth_value, param_values, state.getInstances()); + } + return {success, truth_value, 0, std::move(param_values)}; } case plansys2_msgs::msg::Node::NOT: { - return evaluate( - tree, problem_client, predicates, functions, apply, use_state, - tree.nodes[node_id].children[0], - !negate); + return std::move(evaluate(tree, state, current_node.children[0], !negate)); } case plansys2_msgs::msg::Node::PREDICATE: { bool success = true; bool value = true; + std::vector> param_values; - if (apply) { - if (use_state) { - auto it = - std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - tree.nodes[node_id], true)); - if (negate) { - if (it != predicates.end()) { - predicates.erase(it); - } - value = false; - } else { - if (it == predicates.end()) { - predicates.push_back(tree.nodes[node_id]); - } - } - } else { - if (negate) { - success = success && problem_client->removePredicate(tree.nodes[node_id]); - value = false; - } else { - success = success && problem_client->addPredicate(tree.nodes[node_id]); - } - } - } else { - // negate | exist | output - // F | F | F - // F | T | T - // T | F | T - // T | T | F - if (use_state) { - value = negate ^ - (std::find_if( - predicates.begin(), predicates.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - tree.nodes[node_id], true)) != predicates.end()); - } else { - value = negate ^ problem_client->existPredicate(tree.nodes[node_id]); - } + std::tie(value, param_values) = + unifyPredicate(current_node, state.getUnionPredicatesInferredPredicates()); + if (negate) { + std::tie(value, param_values) = + negateResult(current_node, value, param_values, state.getInstances()); } - - return std::make_tuple(success, value, 0); + return {success, value, 0, std::move(param_values)}; } case plansys2_msgs::msg::Node::FUNCTION: { bool success = true; double value = 0; + std::vector> param_values; - if (use_state) { - auto it = - std::find_if( - functions.begin(), functions.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - tree.nodes[node_id], true)); - if (it != functions.end()) { - value = it->value; - } else { - success = false; - } + auto it = state.getFunction(current_node); + if (it != state.getFunctions().end()) { + value = it->value; } else { - std::optional func = - problem_client->getFunction(parser::pddl::toString(tree, node_id)); - - if (func.has_value()) { - value = func.value().value; - } else { - success = false; - } + success = false; } - - return std::make_tuple(success, false, value); + return {success, false, value, std::move(param_values)}; } case plansys2_msgs::msg::Node::EXPRESSION: { - std::tuple left = evaluate( - tree, problem_client, predicates, - functions, apply, use_state, tree.nodes[node_id].children[0], negate); - std::tuple right = evaluate( - tree, problem_client, predicates, - functions, apply, use_state, tree.nodes[node_id].children[1], negate); - - if (!std::get<0>(left) || !std::get<0>(right)) { - return std::make_tuple(false, false, 0); + auto [left_success, left_value, left_double, left_param_values] = + evaluate(tree, state, current_node.children[0], negate); + auto [right_success, right_value, right_double, right_param_values] = + evaluate(tree, state, current_node.children[1], negate); + + std::vector> param_values; + + if (!left_success || !right_success) { + return {false, false, 0, {}}; } - switch (tree.nodes[node_id].expression_type) { + switch (current_node.expression_type) { case plansys2_msgs::msg::Node::COMP_GE: - if (std::get<2>(left) >= std::get<2>(right)) { - return std::make_tuple(true, negate ^ true, 0); + if (left_double >= right_double) { + return {true, static_cast(negate ^ true), 0, {}}; } else { - return std::make_tuple(true, negate ^ false, 0); + return {true, static_cast(negate ^ false), 0, {}}; } break; case plansys2_msgs::msg::Node::COMP_GT: - if (std::get<2>(left) > std::get<2>(right)) { - return std::make_tuple(true, negate ^ true, 0); + if (left_double > right_double) { + return {true, static_cast(negate ^ true), 0, {}}; } else { - return std::make_tuple(true, negate ^ false, 0); + return {true, static_cast(negate ^ false), 0, {}}; } break; case plansys2_msgs::msg::Node::COMP_LE: - if (std::get<2>(left) <= std::get<2>(right)) { - return std::make_tuple(true, negate ^ true, 0); + if (left_double <= right_double) { + return {true, static_cast(negate ^ true), 0, {}}; } else { - return std::make_tuple(true, negate ^ false, 0); + return {true, static_cast(negate ^ false), 0, {}}; } break; case plansys2_msgs::msg::Node::COMP_LT: - if (std::get<2>(left) < std::get<2>(right)) { - return std::make_tuple(true, negate ^ true, 0); + if (left_double < right_double) { + return {true, static_cast(negate ^ true), 0, {}}; } else { - return std::make_tuple(true, negate ^ false, 0); + return {true, static_cast(negate ^ false), 0, {}}; } break; case plansys2_msgs::msg::Node::COMP_EQ: { auto c_t = plansys2_msgs::msg::Node::CONSTANT; auto p_t = plansys2_msgs::msg::Node::PARAMETER; auto n_t = plansys2_msgs::msg::Node::NUMBER; - auto c0 = tree.nodes[tree.nodes[node_id].children[0]]; - auto c1 = tree.nodes[tree.nodes[node_id].children[1]]; - auto c0_type = c0.node_type; - auto c1_type = c1.node_type; - if ((c0_type == c_t || c0_type == p_t) && (c1_type == c_t || c1_type == p_t)) { - std::string c0_name = (c0_type == p_t) ? c0.parameters[0].name : c0.name; - std::string c1_name = (c1_type == p_t) ? c1.parameters[0].name : c1.name; - return std::make_tuple( - true, - negate ^ ( c1_name == c0_name), - 0); + + const auto & c0 = tree.nodes[current_node.children[0]]; + const auto & c1 = tree.nodes[current_node.children[1]]; + + const auto c0_type = c0.node_type; + const auto c1_type = c1.node_type; + + if ((c0_type == c_t && c1_type == p_t) || (c0_type == p_t && c1_type == c_t)) { + param_values = (c0_type == c_t) ? + mergeParamsValuesVector({{{c1.name, c0.name}}}, right_param_values) : + mergeParamsValuesVector(left_param_values, {{{c0.name, c1.name}}}); + + bool result = !param_values.empty(); + if (negate) { + plansys2_msgs::msg::Node aux_node; + aux_node.parameters.push_back( + c0_type == + p_t ? c0.parameters[0] : c1.parameters[0]); + std::tie(result, param_values) = + negateResult(aux_node, result, param_values, state.getInstances()); + } + return {true, result, 0, std::move(param_values)}; + } + + if (c0_type == p_t && c1_type == p_t) { + std::vector> new_param_values; + new_param_values.reserve(right_param_values.size()); + for (const auto & right_param_value : right_param_values) { + new_param_values.push_back({{c0.name, right_param_value.at(c1.name)}}); + } + param_values = mergeParamsValuesVector(left_param_values, new_param_values); + for (auto & param_value : param_values) { + param_value[c1.name] = param_value[c0.name]; + } + bool result = !param_values.empty(); + if (negate) { + plansys2_msgs::msg::Node aux_node; + aux_node.parameters.push_back(c0.parameters[0]); + aux_node.parameters.push_back(c1.parameters[0]); + std::tie(result, param_values) = + negateResult(aux_node, result, param_values, state.getInstances()); + } + return {true, result, 0, std::move(param_values)}; } + + if (c0_type == c_t && c1_type == c_t) { + return {true, static_cast(negate ^ (c0.name == c1.name)), 0, {}}; + } + if (c0_type == n_t && c1_type == n_t) { - return std::make_tuple(true, negate ^ std::get<2>(left) == std::get<2>(right), 0); + return {true, static_cast(negate ^ (left_double == right_double)), 0, {}}; } break; } case plansys2_msgs::msg::Node::ARITH_MULT: - return std::make_tuple(true, false, std::get<2>(left) * std::get<2>(right)); + return {true, false, left_double * right_double, {}}; break; case plansys2_msgs::msg::Node::ARITH_DIV: - if (std::abs(std::get<2>(right)) > 1e-5) { - return std::make_tuple(true, false, std::get<2>(left) / std::get<2>(right)); + if (std::abs(right_double) > 1e-5) { + return {true, false, left_double / right_double, {}}; } else { // Division by zero not allowed. - return std::make_tuple(false, false, 0); + return {false, false, 0, {}}; } break; case plansys2_msgs::msg::Node::ARITH_ADD: - return std::make_tuple(true, false, std::get<2>(left) + std::get<2>(right)); + return {true, false, left_double + right_double, {}}; break; case plansys2_msgs::msg::Node::ARITH_SUB: - return std::make_tuple(true, false, std::get<2>(left) - std::get<2>(right)); + return {true, false, left_double - right_double, {}}; break; default: break; } - return std::make_tuple(false, false, 0); + return {false, false, 0., {}}; } case plansys2_msgs::msg::Node::FUNCTION_MODIFIER: { - std::tuple left = evaluate( - tree, problem_client, predicates, - functions, apply, use_state, tree.nodes[node_id].children[0], negate); - std::tuple right = evaluate( - tree, problem_client, - predicates, functions, apply, use_state, tree.nodes[node_id].children[1], - negate); - - if (!std::get<0>(left) || !std::get<0>(right)) { - return std::make_tuple(false, false, 0); + auto [left_success, left_value, left_double, left_param_values] = + evaluate(tree, state, current_node.children[0], negate); + auto [right_success, right_value, right_double, right_param_values] = + evaluate(tree, state, current_node.children[1], negate); + + if (!left_success || !right_success) { + return {false, false, 0, {}}; } bool success = true; double value = 0; - switch (tree.nodes[node_id].modifier_type) { + switch (current_node.modifier_type) { case plansys2_msgs::msg::Node::ASSIGN: - value = std::get<2>(right); + value = right_double; break; case plansys2_msgs::msg::Node::INCREASE: - value = std::get<2>(left) + std::get<2>(right); + value = left_double + right_double; break; case plansys2_msgs::msg::Node::DECREASE: - value = std::get<2>(left) - std::get<2>(right); + value = left_double - right_double; break; case plansys2_msgs::msg::Node::SCALE_UP: - value = std::get<2>(left) * std::get<2>(right); + value = left_double * right_double; break; case plansys2_msgs::msg::Node::SCALE_DOWN: // Division by zero not allowed. - if (std::abs(std::get<2>(right)) > 1e-5) { - value = std::get<2>(left) / std::get<2>(right); + if (std::abs(right_double) > 1e-5) { + value = left_double / right_double; } else { success = false; } @@ -287,170 +733,208 @@ std::tuple evaluate( break; } - if (success && apply) { - uint8_t left_id = tree.nodes[node_id].children[0]; - if (use_state) { - auto it = - std::find_if( - functions.begin(), functions.end(), - std::bind( - &parser::pddl::checkNodeEquality, std::placeholders::_1, - tree.nodes[left_id], true)); - if (it != functions.end()) { - it->value = value; - } else { - success = false; - } - } else { - std::stringstream ss; - ss << "(= " << parser::pddl::toString(tree, left_id) << " " << value << ")"; - problem_client->updateFunction(parser::pddl::fromStringFunction(ss.str())); - } - } - - return std::make_tuple(success, false, value); + return {success, false, value, {}}; } case plansys2_msgs::msg::Node::NUMBER: { - return std::make_tuple(true, true, tree.nodes[node_id].value); + return {true, true, current_node.value, {}}; } case plansys2_msgs::msg::Node::CONSTANT: { - if (tree.nodes[node_id].name.size() > 0) { - return std::make_tuple(true, true, 0); + if (current_node.name.size() > 0) { + return {true, true, 0, {}}; } - return std::make_tuple(true, false, 0); + return {true, false, 0, {}}; } case plansys2_msgs::msg::Node::PARAMETER: { - if (tree.nodes[node_id].parameters.size() > 0 && - tree.nodes[node_id].parameters[0].name.front() != '?') - { - return std::make_tuple(true, true, 0); + std::vector> param_values; + auto current_parameter = current_node.parameters[0]; + if (current_parameter.name.front() != '?') { + std::map param_value = { + {current_node.name, current_parameter.name}}; + param_values.emplace_back(param_value); + return {true, true, 0, std::move(param_values)}; } + for (const auto & instance : state.getInstances()) { + if (parser::pddl::checkParamTypeEquivalence(current_parameter, instance)) { + std::map param_value = { + {current_parameter.name, instance.name}}; + param_values.emplace_back(param_value); + } + } + return {true, false, 0, std::move(param_values)}; } case plansys2_msgs::msg::Node::EXISTS: { - std::vector instances; - if (use_state == false) { - instances = problem_client->getInstances(); - } else { - for (auto predicate : predicates) { - std::for_each( - predicate.parameters.begin(), predicate.parameters.end(), - [&](auto p) { - if (std::find(instances.begin(), instances.end(), p) == instances.end()) { - instances.push_back(p); - } - }); - } - } - std::vector> parameters_vector; - for (size_t i = 0; i < tree.nodes[node_id].parameters.size(); i++) { - std::vector p_vector; - std::for_each( - instances.begin(), instances.end(), - [&](auto i) {p_vector.push_back(i.name);}); - parameters_vector.push_back(p_vector); + auto [success, truth_value, _, param_values] = + evaluate(tree, state, current_node.children[0], false); + auto free_params = get_node_children_free_parameters(tree, current_node); + + // 1. Convert free_params to a set of allowed names + std::unordered_set free_param_names; + for (const auto & param : free_params) { + free_param_names.insert(param.name); } - std::vector> possible_parameters_values; - std::vector aux; - plansys2::cart_product( - possible_parameters_values, aux, parameters_vector.begin(), parameters_vector.end()); - - for (auto parameters_values : possible_parameters_values) { - std::map replace; - for (size_t i = 0; i < tree.nodes[node_id].parameters.size(); i++) { - replace[tree.nodes[node_id].parameters[i].name] = parameters_values[i]; + // 2. Remove keys not in free_params from each map in param_values + for (auto it = param_values.begin(); it != param_values.end(); ) { + // Remove keys not in free_param_names + for (auto mit = it->begin(); mit != it->end(); ) { + if (free_param_names.find(mit->first) == free_param_names.end()) { + mit = it->erase(mit); + } else { + ++mit; + } } - auto tree_replaced = plansys2::replace_children_param(tree, node_id, replace); - std::tuple result = evaluate( - tree_replaced, - problem_client, - predicates, - functions, - apply, - use_state, - tree_replaced.nodes[node_id].children[0], - negate); - if (std::get<1>(result)) { - return result; + // If the map is empty after erasing, remove it from param_values + if (it->empty()) { + it = param_values.erase(it); + } else { + ++it; } } - return std::make_tuple(true, false, 0); + if (negate) { + std::tie(truth_value, param_values) = + negateResult(free_params, truth_value, param_values, state.getInstances()); + } + return {success, truth_value, 0, std::move(param_values)}; } default: - std::cerr << "evaluate: Error parsing expresion [" << - parser::pddl::toString(tree, node_id) << "]" << std::endl; + std::cerr << "evaluate: Error parsing expresion [" << parser::pddl::toString(tree, node_id) + << "]" << std::endl; } - - return std::make_tuple(false, false, 0); + return {false, false, 0, {}}; } -std::tuple evaluate( +std::tuple>> evaluate( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - bool apply, - uint32_t node_id) + std::shared_ptr problem_client, uint32_t node_id, bool negate) { - std::vector predicates; - std::vector functions; - return evaluate(tree, problem_client, predicates, functions, apply, false, node_id); -} - -std::tuple evaluate( - const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - bool apply, - uint32_t node_id) -{ - std::shared_ptr problem_client; - return evaluate(tree, problem_client, predicates, functions, apply, true, node_id); + plansys2::State state = problem_client->getState(); + return evaluate(tree, state, node_id, negate); } bool check( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - uint32_t node_id) + std::shared_ptr problem_client, uint32_t node_id, bool negate) { - std::tuple ret = evaluate(tree, problem_client, false, node_id); + std::tuple>> ret = + evaluate(tree, problem_client, node_id, negate); return std::get<1>(ret); } bool check( - const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - uint32_t node_id) + const plansys2_msgs::msg::Tree & tree, const plansys2::State & state, uint32_t node_id, + bool negate) { - std::tuple ret = evaluate(tree, predicates, functions, false, node_id); - + std::tuple>> ret = + evaluate(tree, state, node_id, negate); return std::get<1>(ret); } bool apply( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - uint32_t node_id) + std::shared_ptr problem_client, uint32_t node_id, bool negate, + bool derive) { - std::tuple ret = evaluate(tree, problem_client, true, node_id); + plansys2::State state; + std::vector nodes_modified; + return apply(tree, problem_client, state, nodes_modified, false, node_id, negate, derive); +} - return std::get<0>(ret); +bool apply( + const plansys2_msgs::msg::Tree & tree, plansys2::State & state, uint32_t node_id, bool negate, + bool derive) +{ + std::shared_ptr problem_client; + std::vector nodes_modified; + return apply(tree, problem_client, state, nodes_modified, true, node_id, negate, derive); +} + +bool apply( + const plansys2_msgs::msg::Tree & tree, plansys2::State & state, + std::vector & nodes_modified, uint32_t node_id, bool negate, + bool derive) +{ + std::shared_ptr problem_client; + return apply(tree, problem_client, state, nodes_modified, true, node_id, negate, derive); } bool apply( const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - uint32_t node_id) + std::shared_ptr problem_client, plansys2::State & state, + std::vector & nodes_modified, bool use_state, uint32_t node_id, + bool negate, bool derive) { - std::tuple ret = evaluate(tree, predicates, functions, true, node_id); + if (tree.nodes.empty()) { + return true; + } + + bool success = true; + const auto & current_node = tree.nodes[node_id]; + switch (current_node.node_type) { + case plansys2_msgs::msg::Node::AND: { + for (const auto & child_id : current_node.children) { + bool child_success = + apply(tree, problem_client, state, nodes_modified, use_state, child_id, negate, false); + success &= child_success; + } + break; + } + + case plansys2_msgs::msg::Node::NOT: { + success = apply( + tree, problem_client, state, nodes_modified, use_state, current_node.children[0], !negate, + false); + break; + } - return std::get<0>(ret); + case plansys2_msgs::msg::Node::PREDICATE: { + if (use_state) { + success &= + negate ? state.removePredicate(current_node) : state.addPredicate(current_node); + } else { + success &= negate ? problem_client->removePredicate(current_node) : + problem_client->addPredicate(current_node); + } + nodes_modified.push_back(current_node); + break; + } + + case plansys2_msgs::msg::Node::FUNCTION_MODIFIER: { + auto [eval_success, eval_truth, eval_value, eval_param_values] = + std::make_tuple(false, false, 0.0, std::vector>{}); + if (use_state) { + std::tie(eval_success, eval_truth, eval_value, eval_param_values) = + evaluate(tree, state, node_id, negate); + } else { + std::tie(eval_success, eval_truth, eval_value, eval_param_values) = + evaluate(tree, problem_client, node_id, negate); + } + if (eval_success) { + uint8_t left_id = current_node.children[0]; + if (use_state) { + success = state.updateFunctionValue(tree.nodes[left_id], eval_value); + } else { + std::stringstream ss; + ss << "(= " << parser::pddl::toString(tree, left_id) << " " << eval_value << ")"; + problem_client->updateFunction(parser::pddl::fromStringFunction(ss.str())); + } + } + break; + } + default: + success = false; + std::cerr << "Apply: Error parsing expresion [" << parser::pddl::toString(tree, node_id) + << "]" << std::endl; + } + if (derive && use_state) { + solveDerivedPredicates(state, nodes_modified); + } + return success; } std::pair parse_action(const std::string & input) @@ -465,7 +949,7 @@ std::pair parse_action(const std::string & input) } action.erase(0, 1); // remove initial ( - action.pop_back(); // remove last ) + action.pop_back(); // remove last ) return std::make_pair(action, time); } @@ -503,8 +987,7 @@ std::vector get_action_params(const std::string & input) size_t start = 0, end = 0; while (end != std::string::npos) { end = expr.find(" ", start); - auto param = expr.substr( - start, (end == std::string::npos) ? std::string::npos : end - start); + auto param = expr.substr(start, (end == std::string::npos) ? std::string::npos : end - start); ret.push_back(param); start = ((end > (std::string::npos - 1)) ? std::string::npos : end + 1); } @@ -512,44 +995,4 @@ std::vector get_action_params(const std::string & input) return ret; } -plansys2_msgs::msg::Tree replace_children_param( - const plansys2_msgs::msg::Tree & tree, - const uint8_t & node_id, - const std::map & replace) -{ - plansys2_msgs::msg::Tree new_tree = tree; - if (tree.nodes[node_id].children.size() > 0) { - for (auto & child_id : tree.nodes[node_id].children) { - new_tree = replace_children_param(new_tree, child_id, replace); - } - } - - for (size_t i = 0; i < tree.nodes[node_id].parameters.size(); i++) { - if (replace.find(tree.nodes[node_id].parameters[i].name) != replace.end()) { - new_tree.nodes[node_id].parameters[i].name = replace.at( - tree.nodes[node_id].parameters[i].name); - } - } - return new_tree; -} - -void cart_product( - std::vector> & rvvi, // final result - std::vector & rvi, // current result - std::vector>::const_iterator me, // current input - std::vector>::const_iterator end) // final input -{ - if (me == end) { - rvvi.push_back(rvi); - return; - } - - const std::vector & mevi = *me; - for (std::vector::const_iterator it = mevi.begin(); it != mevi.end(); it++) { - rvi.push_back(*it); - cart_product(rvvi, rvi, me + 1, end); - rvi.pop_back(); - } -} - } // namespace plansys2 diff --git a/plansys2_problem_expert/test/pddl/domain_derived.pddl b/plansys2_problem_expert/test/pddl/domain_derived.pddl new file mode 100644 index 000000000..7f6b328ef --- /dev/null +++ b/plansys2_problem_expert/test/pddl/domain_derived.pddl @@ -0,0 +1,171 @@ +(define (domain simple) +(:requirements :strips :typing :adl :derived-predicates :fluents) + +;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +(:types + person - object + robot - object + room - object +);; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; + +(:constants drone123 - robot drone_area - room) + +;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; +(:predicates + (party ?p ?p2 ?p3 ?p4 ?p5 ?r ?r2 ?room) + (robot_at ?r - robot ?ro - room) + (person_at ?p - person ?ro - room) + (is_drone ?r - robot) + (is_rocket ?r) + (inferred-robot_at ?r - robot ?ro - room) + (inferred-exists-another-drone-at-drone_area ?r - robot) + (inferred-person_at ?p - person ?ro - room) + (inferred-party ?p ?p2 ?p3 ?p4 ?p5 ?r ?r2 ?room) + (inferred-exists-party-in-room ?room) + (inferred-aerial ?r - robot) + (inferred-not_aerial ?r - robot) + (inferred-drone_has_battery_level ?r - robot) + (inferred-drone123 ?r - robot) + (inferred-not-drone123 ?r - robot) + (inferred-same-drone ?r ?r2 - robot) + (inferred-not-same-drone ?r ?r2 - robot) + (inferred-exists-equal-drone ?r) + (inferred-exists-another-drone ?r) +);; end Predicates ;;;;;;;;;;;;;;;;;;;; + +(:functions + (battery_level ?r - robot) +) + +;; Derived predicates ;;;;;;;;;;;;;;;;;;;;;;;;;;;; +(:derived (inferred-robot_at ?r - robot ?ro - room) + (and + (robot_at ?r ?ro) + ) +) +(:derived (inferred-exists-another-drone-at-drone_area ?r - robot) + (and + (is_drone ?r) + (exists (?r) + (and + (robot_at ?r drone_area) + (not (= ?r drone123)) + ) + ) + ) +) + +(:derived (inferred-person_at ?p - person ?ro - room) + (and + (person_at ?p ?ro) + ) +) + +(:derived (inferred-aerial ?r - robot) + (or + (is_drone ?r) + (is_rocket ?r) + ) +) + +(:derived (inferred-not_aerial ?r - robot) + (and + (not (is_drone ?r)) + (not (is_rocket ?r)) + ) +) + +(:derived (inferred-party ?p ?p2 ?p3 ?p4 ?p5 ?r ?r2 ?room) + (and + (inferred-person_at ?p ?room) + (inferred-person_at ?p2 ?room) + (inferred-person_at ?p3 ?room) + (inferred-person_at ?p4 ?room) + (inferred-person_at ?p5 ?room) + (inferred-robot_at ?r ?room) + (inferred-robot_at ?r2 ?room) + ) +) + +(:derived (inferred-exists-party-in-room ?room) + (and + (exists (?p ?p2 ?p3 ?p4 ?p5 ?r ?r2) + (and + (inferred-party ?p ?p2 ?p3 ?p4 ?p5 ?r ?r2 ?room) + ) + ) + ) +) + +(:derived (inferred-drone_has_battery_level ?r - robot) + (and + (= (battery_level ?r) 99) + (is_drone ?r) + ) +) + +(:derived (inferred-drone123 ?r - robot) + (and + (= ?r drone123) + (is_drone ?r) + ) +) + +(:derived (inferred-not-drone123 ?r - robot) + (and + (not (= ?r drone123)) + (is_drone ?r) + ) +) + +(:derived (inferred-same-drone ?r ?r2 - robot) + (and + (is_drone ?r) + (is_drone ?r2) + (= ?r ?r2) + ) +) + +(:derived (inferred-not-same-drone ?r ?r2 - robot) + (and + (is_drone ?r) + (is_drone ?r2) + (not (= ?r ?r2)) + ) +) + +(:derived (inferred-exists-equal-drone ?r) + (and + (exists (?r2) + (and + (inferred-same-drone ?r ?r2) + ) + ) + ) +) + +(:derived (inferred-exists-another-drone ?r) + (and + (exists (?r2) + (and + (is_drone ?r) + (is_drone ?r2) + (not (= ?r ?r2)) + ) + ) + ) +) + +;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; +(:action party + :parameters (?p ?p2 ?p3 ?p4 ?p5 ?r ?r2 ?room) + :precondition (and + (inferred-party ?p ?p2 ?p3 ?p4 ?p5 ?r ?r2 ?room) + ) + :effect (and + (party ?p ?p2 ?p3 ?p4 ?p5 ?r ?r2 ?room) + ) +) + + +);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; \ No newline at end of file diff --git a/plansys2_problem_expert/test/pddl/domain_exists.pddl b/plansys2_problem_expert/test/pddl/domain_exists.pddl index 3d61e1b35..7f262f56c 100644 --- a/plansys2_problem_expert/test/pddl/domain_exists.pddl +++ b/plansys2_problem_expert/test/pddl/domain_exists.pddl @@ -14,7 +14,7 @@ (:constants rob1 rob2 - robot - bedroom bathroom - room + bathroom - room ) (:functions diff --git a/plansys2_problem_expert/test/pddl/domain_simple_derived.pddl b/plansys2_problem_expert/test/pddl/domain_simple_derived.pddl deleted file mode 100644 index 6a2a98803..000000000 --- a/plansys2_problem_expert/test/pddl/domain_simple_derived.pddl +++ /dev/null @@ -1,91 +0,0 @@ -(define (domain simple) -(:requirements :strips :typing :adl :fluents :durative-actions :derived-predicates) - -;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(:types -person - object -message - object -robot - object -room - object -room_with_teleporter - room -);; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; - -;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; -(:predicates - -(robot_talk ?r - robot ?m - message ?p - person) -(robot_near_person ?r - robot ?p - person) -(robot_at ?r - robot ?ro - room) -(person_at ?p - person ?ro - room) -(is_teleporter_enabled ?r - room_with_teleporter) -(is_teleporter_destination ?r - room) -(inferred-robot_at ?r - robot ?ro - room) -(inferred-person_at ?p - person ?ro - room) -);; end Predicates ;;;;;;;;;;;;;;;;;;;; -;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; -(:functions - (room_distance ?r1 - room ?r2 - room) -);; end Functions ;;;;;;;;;;;;;;;;;;;; - -;; Derived predicates ;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(:derived (inferred-robot_at ?r - robot ?ro - room) - (and - (robot_at ?r ?ro) - ) -) -(:derived (inferred-person_at ?p - person ?ro - room) - (and - (person_at ?p ?ro) - ) -) - -;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(:durative-action move - :parameters (?r - robot ?r1 ?r2 - room) - :duration ( = ?duration (* (room_distance ?r1 ?r2) 0.5)) - :condition (and - (at start(robot_at ?r ?r1))) - :effect (and - (at start(not(robot_at ?r ?r1))) - (at end(robot_at ?r ?r2)) - ) -) - -(:action teleport - :parameters (?r - robot ?r1 - room_with_teleporter ?r2 - room) - :precondition (and - (robot_at ?r ?r1) - (is_teleporter_enabled ?r1) - (is_teleporter_destination ?r2) - ) - :effect (and - (not(robot_at ?r ?r1)) - (robot_at ?r ?r2) - ) -) - -(:durative-action talk - :parameters (?r - robot ?from ?p - person ?m - message) - :duration ( = ?duration 5) - :condition (and - (over all(robot_near_person ?r ?p)) - ) - :effect (and - (at end(robot_talk ?r ?m ?p)) - ) -) - -(:durative-action approach - :parameters (?r - robot ?ro - room ?p - person) - :duration ( = ?duration 5) - :condition (and - (over all(robot_at ?r ?ro)) - (over all(person_at ?p ?ro)) - (at end(person_at ?p ?ro)) - ) - :effect (and - (at end(robot_near_person ?r ?p)) - ) -) - -);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; diff --git a/plansys2_problem_expert/test/pddl/problem_derived.pddl b/plansys2_problem_expert/test/pddl/problem_derived.pddl new file mode 100644 index 000000000..fb31bd905 --- /dev/null +++ b/plansys2_problem_expert/test/pddl/problem_derived.pddl @@ -0,0 +1,47 @@ +(define (problem simple_1) + (:domain simple) + (:objects + leia r2d2 turtlebot mirte tiago walle pluto donald rob1 eva rob2 - robot + person1 person2 person3 person4 person5 boringperson boringperson2 boringperson3 boringperson4 boringperson5 Jack Alice bob jhon jose carlos marta dilma edson einstein albert lucas gustavo person6 person7 person8 person9 person10 - person + kitchen bedroom livingroom garden office pool garage - room + ) + (:init + (robot_at leia kitchen) + (robot_at r2d2 bedroom) + (robot_at turtlebot livingroom) + (robot_at mirte garden) + (robot_at tiago office) + (robot_at walle pool) + (robot_at pluto garage) + (robot_at donald kitchen) + (robot_at rob1 bedroom) + (robot_at eva kitchen) + (robot_at rob2 livingroom) + (robot_at drone123 drone_area) + (person_at Jack bedroom) + (person_at Alice bedroom) + (person_at bob livingroom) + (person_at Jhon livingroom) + (person_at jose livingroom) + (person_at carlos livingroom) + (person_at marta livingroom) + (person_at dilma livingroom) + (person_at edson livingroom) + (person_at einstein livingroom) + (person_at albert livingroom) + (person_at lucas livingroom) + (person_at gustavo livingroom) + (is_drone rob1) + (is_drone drone123) + (is_rocket rob2) + + (= (battery_level r2d2) 99) + (= (battery_level rob1) 50) + ) + + ;; The goal is to have both packages delivered to their destinations: + (:goal (and + (party gustavo lucas albert einstein edson turtlebot rob2 livingroom) + ) + ) +) diff --git a/plansys2_problem_expert/test/pddl/problem_simple_exists.pddl b/plansys2_problem_expert/test/pddl/problem_simple_exists.pddl new file mode 100644 index 000000000..1756594d7 --- /dev/null +++ b/plansys2_problem_expert/test/pddl/problem_simple_exists.pddl @@ -0,0 +1,13 @@ +(define (problem simple_1) + (:domain simple) + (:objects + leia - robot + kitchen bedroom - room + ) + (:init + (robot_at leia kitchen) + ) + + ;; The goal is to have both packages delivered to their destinations: + (:goal (and)) +) \ No newline at end of file diff --git a/plansys2_problem_expert/test/pddl/suave_domain.pddl b/plansys2_problem_expert/test/pddl/suave_domain.pddl new file mode 100644 index 000000000..e9031c730 --- /dev/null +++ b/plansys2_problem_expert/test/pddl/suave_domain.pddl @@ -0,0 +1,1006 @@ +(define (domain suave) + (:requirements + :strips + :typing + :adl + :negative-preconditions + :disjunctive-preconditions + :derived-predicates + :existential-preconditions + ) + + (:types + pipeline + robot + action + ) + + (:constants a_search_pipeline a_inspect_pipeline - action + INTERNAL_ERROR_string IN_ERROR_FR_string IN_ERROR_COMPONENT_string IN_ERROR_NFR_string FALSE_string false_boolean water_visibility RECOVERED_string true_boolean fd_unground f_follow_pipeline f_generate_search_path f_maintain_motion + ) + + (:predicates + (pipeline_found ?p - pipeline) + (pipeline_inspected ?p - pipeline) + + (robot_started ?r - robot) + + (action_requires ?a - action ?f1 ?f2) + (fd_available ?fd) + (system_in_mode ?s ?m) + (inferred-f_activated ?f) + + (inferred-Component ?x) + (Component ?x) + (inferred-Function ?x) + (Function ?x) + (inferred-FunctionDesign ?x) + (FunctionDesign ?x) + (inferred-FunctionGrounding ?x) + (FunctionGrounding ?x) + (inferred-Objective ?x) + (Objective ?x) + (inferred-QAvalue ?x) + (QAvalue ?x) + (inferred-QualityAttributeType ?x) + (QualityAttributeType ?x) + (inferred-Fd_error_log ?x ?y) + (fd_error_log ?x ?y) + (inferred-HasNFR ?x ?y) + (hasNFR ?x ?y) + (inferred-HasQAestimation ?x ?y) + (hasQAestimation ?x ?y) + (inferred-HasQAvalue ?x ?y) + (hasQAvalue ?x ?y) + (inferred-IsQAtype ?x ?y) + (isQAtype ?x ?y) + (inferred-RequiresC ?x ?y) + (requiresC ?x ?y) + (inferred-RequiresO ?x ?y) + (requiresO ?x ?y) + (inferred-SolvesF ?x ?y) + (solvesF ?x ?y) + (inferred-SolvesO ?x ?y) + (solvesO ?x ?y) + (inferred-TypeF ?x ?y) + (typeF ?x ?y) + (inferred-TypeFD ?x ?y) + (typeFD ?x ?y) + (inferred-C_status ?x ?y) + (c_status ?x ?y) + (inferred-Fd_efficacy ?x ?y) + (fd_efficacy ?x ?y) + (inferred-Fd_realisability ?x ?y) + (fd_realisability ?x ?y) + (inferred-Fg_status ?x ?y) + (fg_status ?x ?y) + (inferred-HasValue ?x ?y) + (hasValue ?x ?y) + (inferred-O_always_improve ?x ?y) + (o_always_improve ?x ?y) + (inferred-O_status ?x ?y) + (o_status ?x ?y) + (inferred-O_updatable ?x ?y) + (o_updatable ?x ?y) + (inferred-Qa_comparison_operator ?x ?y) + (qa_comparison_operator ?x ?y) + (inferred-Qa_critical ?x ?y) + (qa_critical ?x ?y) + (inferred-Inconsistent) + (inferred-LessThan ?x ?y) + ) + + (:derived (inferred-f_activated ?f) + (and + (Function ?f) + (exists (?fd) + (and + (FunctionDesign ?fd) + (system_in_mode ?f ?fd) + (not (= ?fd fd_unground)) + ) + ) + ) + ) + + (:derived (inferred-Component ?x) + (and + (Component ?x) + ) + ) + + + (:derived (inferred-Function ?x) + (and + (Function ?x) + ) + ) + + + (:derived (inferred-FunctionDesign ?x) + (and + (FunctionDesign ?x) + ) + ) + + + (:derived (inferred-FunctionGrounding ?x) + (and + (FunctionGrounding ?x) + ) + ) + + + (:derived (inferred-Objective ?x) + (and + (Objective ?x) + ) + ) + + + (:derived (inferred-QAvalue ?x) + (and + (QAvalue ?x) + ) + ) + + + (:derived (inferred-QualityAttributeType ?x) + (and + (QualityAttributeType ?x) + ) + ) + + + (:derived (inferred-Fd_error_log ?x ?y) + (and + (fd_error_log ?x ?y) + ) + ) + + + (:derived (inferred-HasNFR ?x ?y) + (and + (hasNFR ?x ?y) + ) + ) + + + (:derived (inferred-HasQAestimation ?x ?y) + (and + (hasQAestimation ?x ?y) + ) + ) + + + (:derived (inferred-HasQAvalue ?x ?y) + (and + (hasQAvalue ?x ?y) + ) + ) + + + (:derived (inferred-IsQAtype ?x ?y) + (and + (isQAtype ?x ?y) + ) + ) + + + (:derived (inferred-RequiresC ?x ?y) + (and + (requiresC ?x ?y) + ) + ) + + + (:derived (inferred-RequiresO ?x ?y) + (and + (requiresO ?x ?y) + ) + ) + + + (:derived (inferred-SolvesF ?x ?y) + (and + (solvesF ?x ?y) + ) + ) + + + (:derived (inferred-SolvesO ?x ?y) + (and + (solvesO ?x ?y) + ) + ) + + + (:derived (inferred-TypeF ?x ?y) + (and + (typeF ?x ?y) + ) + ) + + + (:derived (inferred-TypeFD ?x ?y) + (and + (typeFD ?x ?y) + ) + ) + + + (:derived (inferred-C_status ?x ?y) + (and + (c_status ?x ?y) + ) + ) + + + (:derived (inferred-Fd_efficacy ?x ?y) + (and + (fd_efficacy ?x ?y) + ) + ) + + + (:derived (inferred-Fd_realisability ?x ?y) + (and + (fd_realisability ?x ?y) + ) + ) + + + (:derived (inferred-Fg_status ?x ?y) + (and + (fg_status ?x ?y) + ) + ) + + + (:derived (inferred-HasValue ?x ?y) + (and + (hasValue ?x ?y) + ) + ) + + + (:derived (inferred-O_always_improve ?x ?y) + (and + (o_always_improve ?x ?y) + ) + ) + + + (:derived (inferred-O_status ?x ?y) + (and + (o_status ?x ?y) + ) + ) + + + (:derived (inferred-O_updatable ?x ?y) + (and + (o_updatable ?x ?y) + ) + ) + + + (:derived (inferred-Qa_comparison_operator ?x ?y) + (and + (qa_comparison_operator ?x ?y) + ) + ) + + + (:derived (inferred-Qa_critical ?x ?y) + (and + (qa_critical ?x ?y) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-TypeFD ?x ?y) + (inferred-TypeFD ?x ?z) + (= ?y ?z) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-SolvesO ?x ?y) + (inferred-SolvesO ?x ?z) + (= ?y ?z) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-TypeF ?x ?y) + (inferred-TypeF ?x ?z) + (= ?y ?z) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-SolvesF ?x ?y) + (inferred-SolvesF ?x ?z) + (= ?y ?z) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-IsQAtype ?x ?y) + (inferred-IsQAtype ?x ?z) + (= ?y ?z) + ) + ) + ) + + + (:derived (inferred-Function ?y) + (exists (?x) + (and + (inferred-SolvesF ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionDesign ?y) + (exists (?x) + (and + (inferred-TypeFD ?x ?y) + ) + ) + ) + + + (:derived (inferred-Objective ?y) + (exists (?x) + (and + (inferred-Fd_error_log ?x ?y) + ) + ) + ) + + + (:derived (inferred-Objective ?y) + (exists (?x) + (and + (inferred-RequiresO ?x ?y) + ) + ) + ) + + + (:derived (inferred-Objective ?y) + (exists (?x) + (and + (inferred-SolvesO ?x ?y) + ) + ) + ) + + + (:derived (inferred-QAvalue ?y) + (exists (?x) + (and + (inferred-HasQAestimation ?x ?y) + ) + ) + ) + + + (:derived (inferred-Function ?y) + (exists (?x) + (and + (inferred-TypeF ?x ?y) + ) + ) + ) + + + (:derived (inferred-QAvalue ?y) + (exists (?x) + (and + (inferred-HasQAvalue ?x ?y) + ) + ) + ) + + + (:derived (inferred-Component ?y) + (exists (?x) + (and + (inferred-RequiresC ?x ?y) + ) + ) + ) + + + (:derived (inferred-QAvalue ?y) + (exists (?x) + (and + (inferred-HasNFR ?x ?y) + ) + ) + ) + + + (:derived (inferred-QualityAttributeType ?y) + (exists (?x) + (and + (inferred-IsQAtype ?x ?y) + ) + ) + ) + + + (:derived (inferred-QualityAttributeType ?x) + (exists (?y) + (and + (inferred-Qa_comparison_operator ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-Fd_efficacy ?x ?y) + ) + ) + ) + + + (:derived (inferred-Objective ?x) + (exists (?y) + (and + (inferred-O_status ?x ?y) + ) + ) + ) + + + (:derived (inferred-QualityAttributeType ?x) + (exists (?y) + (and + (inferred-Qa_critical ?x ?y) + ) + ) + ) + + + (:derived (inferred-Component ?x) + (exists (?y) + (and + (inferred-C_status ?x ?y) + ) + ) + ) + + + (:derived (inferred-Objective ?x) + (exists (?y) + (and + (inferred-O_always_improve ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-Fd_realisability ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionGrounding ?x) + (exists (?y) + (and + (inferred-Fg_status ?x ?y) + ) + ) + ) + + + (:derived (inferred-Objective ?x) + (exists (?y) + (and + (inferred-O_updatable ?x ?y) + ) + ) + ) + + + (:derived (inferred-QAvalue ?x) + (exists (?y) + (and + (inferred-HasValue ?x ?y) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Qa_comparison_operator ?x ?y) + (inferred-Qa_comparison_operator ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-C_status ?x ?y) + (inferred-C_status ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Fd_efficacy ?x ?y) + (inferred-Fd_efficacy ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-HasValue ?x ?y) + (inferred-HasValue ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-O_updatable ?x ?y) + (inferred-O_updatable ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-O_status ?x ?y) + (inferred-O_status ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-O_always_improve ?x ?y) + (inferred-O_always_improve ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Fg_status ?x ?y) + (inferred-Fg_status ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Fd_realisability ?x ?y) + (inferred-Fd_realisability ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + + (:derived (inferred-Objective ?x) + (exists (?y) + (and + (inferred-HasNFR ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-HasQAestimation ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-SolvesF ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionGrounding ?x) + (exists (?y) + (and + (inferred-HasQAvalue ?x ?y) + ) + ) + ) + + + (:derived (inferred-Objective ?x) + (exists (?y) + (and + (inferred-TypeF ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-RequiresC ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionGrounding ?x) + (exists (?y) + (and + (inferred-RequiresO ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionGrounding ?x) + (exists (?y) + (and + (inferred-SolvesO ?x ?y) + ) + ) + ) + + + (:derived (inferred-QAvalue ?x) + (exists (?y) + (and + (inferred-IsQAtype ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionGrounding ?x) + (exists (?y) + (and + (inferred-TypeFD ?x ?y) + ) + ) + ) + + + (:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-Fd_error_log ?x ?y) + ) + ) + ) + + + (:derived (inferred-Fd_error_log ?fd ?o) + (exists (?fg) + (and + (inferred-TypeFD ?fg ?fd) + (inferred-FunctionGrounding ?fg) + (inferred-SolvesO ?fg ?o) + (inferred-Objective ?o) + (inferred-FunctionDesign ?fd) + (inferred-O_status ?o INTERNAL_ERROR_string) + ) + ) + ) + + + (:derived (inferred-O_status ?o ?IN_ERROR_FR_string) + (and + (= ?IN_ERROR_FR_string IN_ERROR_FR_string) + (exists (?fg) + (and + (inferred-Fg_status ?fg IN_ERROR_FR_string) + (inferred-FunctionGrounding ?fg) + (inferred-SolvesO ?fg ?o) + (inferred-Objective ?o) + ) + ) + ) + ) + + + (:derived (inferred-O_status ?o ?IN_ERROR_COMPONENT_string) + (and + (= ?IN_ERROR_COMPONENT_string IN_ERROR_COMPONENT_string) + (exists (?fg) + (and + (inferred-FunctionGrounding ?fg) + (inferred-SolvesO ?fg ?o) + (inferred-Fg_status ?fg IN_ERROR_COMPONENT_string) + (inferred-Objective ?o) + ) + ) + ) + ) + + + (:derived (inferred-O_status ?o ?IN_ERROR_NFR_string) + (and + (= ?IN_ERROR_NFR_string IN_ERROR_NFR_string) + (exists (?fg) + (and + (inferred-FunctionGrounding ?fg) + (inferred-Fg_status ?fg IN_ERROR_NFR_string) + (inferred-SolvesO ?fg ?o) + (inferred-Objective ?o) + ) + ) + ) + ) + + + (:derived (inferred-Fg_status ?fg ?IN_ERROR_COMPONENT_string) + (and + (= ?IN_ERROR_COMPONENT_string IN_ERROR_COMPONENT_string) + (exists (?c ?fd) + (and + (inferred-FunctionGrounding ?fg) + (inferred-C_status ?c FALSE_string) + (inferred-TypeFD ?fg ?fd) + (inferred-Component ?c) + (inferred-RequiresC ?fd ?c) + ) + ) + ) + ) + + + (:derived (inferred-Fd_realisability ?fd ?false_boolean) + (and + (= ?false_boolean false_boolean) + (exists (?c) + (and + (inferred-C_status ?c FALSE_string) + (inferred-Component ?c) + (inferred-RequiresC ?fd ?c) + ) + ) + ) + ) + + + (:derived (inferred-Fg_status ?fg ?IN_ERROR_FR_string) + (and + (= ?IN_ERROR_FR_string IN_ERROR_FR_string) + (exists (?o) + (and + (inferred-O_status ?o IN_ERROR_FR_string) + (inferred-FunctionGrounding ?fg) + (inferred-RequiresO ?fg ?o) + (inferred-Objective ?o) + ) + ) + ) + ) + + + (:derived (inferred-Fg_status ?fg ?IN_ERROR_NFR_string) + (and + (= ?IN_ERROR_NFR_string IN_ERROR_NFR_string) + (exists (?mqa ?eqa ?mqav ?fd ?eqav) + (and + (inferred-FunctionGrounding ?fg) + (inferred-HasQAvalue ?fg ?mqa) + (inferred-IsQAtype ?eqa water_visibility) + (inferred-HasValue ?mqa ?mqav) + (inferred-TypeFD ?fg ?fd) + (inferred-HasValue ?eqa ?eqav) + (inferred-HasQAestimation ?fd ?eqa) + (inferred-IsQAtype ?mqa water_visibility) + (inferred-FunctionDesign ?fd) + (inferred-QAvalue ?mqa) + (inferred-QAvalue ?eqa) + (inferred-LessThan ?mqav ?eqav) + ) + ) + ) + ) + + + (:derived (inferred-Fg_status ?fg ?IN_ERROR_COMPONENT_string) + (and + (= ?IN_ERROR_COMPONENT_string IN_ERROR_COMPONENT_string) + (exists (?o) + (and + (inferred-FunctionGrounding ?fg) + (inferred-RequiresO ?fg ?o) + (inferred-O_status ?o IN_ERROR_COMPONENT_string) + (inferred-Objective ?o) + ) + ) + ) + ) + + + (:derived (inferred-O_updatable ?o ?true_boolean) + (and + (= ?true_boolean true_boolean) + (exists (?c) + (and + (inferred-C_status ?c RECOVERED_string) + (inferred-Component ?c) + (inferred-Objective ?o) + ) + ) + ) + ) + + + (:derived (inferred-Fd_error_log ?fd ?o) + (exists (?fg) + (and + (inferred-FunctionDesign ?fd) + (inferred-O_status ?o IN_ERROR_FR_string) + (inferred-FunctionGrounding ?fg) + (inferred-TypeFD ?fg ?fd) + (inferred-SolvesO ?fg ?o) + (inferred-Objective ?o) + ) + ) + ) + + + (:derived (inferred-Fg_status ?fg ?IN_ERROR_NFR_string) + (and + (= ?IN_ERROR_NFR_string IN_ERROR_NFR_string) + (exists (?o) + (and + (inferred-FunctionGrounding ?fg) + (inferred-RequiresO ?fg ?o) + (inferred-O_status ?o IN_ERROR_NFR_string) + (inferred-Objective ?o) + ) + ) + ) + ) + + (:action start_robot + :parameters (?r - robot) + :precondition (and + ) + :effect (and + (robot_started ?r) + ) + ) + + (:action reconfigure + :parameters (?f ?fd_initial ?fd_goal) + :precondition (and + (Function ?f) + (FunctionDesign ?fd_initial) + (FunctionDesign ?fd_goal) + (system_in_mode ?f ?fd_initial) + (not (= ?fd_initial ?fd_goal)) + ) + :effect (and + (not (system_in_mode ?f ?fd_initial)) + (system_in_mode ?f ?fd_goal) + ) + ) + + (:action search_pipeline + :parameters (?a - action ?p - pipeline ?r - robot ?fd1 ?fd2) + :precondition (and + (= ?a a_search_pipeline) + (exists (?f1 ?f2) + (and + (action_requires ?a ?f1 ?f2) + (Function ?f1) + (Function ?f2) + (FunctionDesign ?fd1) + (FunctionDesign ?fd2) + (solvesF ?fd1 ?f1) + (solvesF ?fd2 ?f2) + (not (inferred-Fd_realisability ?fd1 false_boolean)) + (not (inferred-Fd_realisability ?fd2 false_boolean)) + (system_in_mode ?f1 ?fd1) + (system_in_mode ?f2 ?fd2) + ) + ) + (not (inferred-f_activated f_follow_pipeline)) + (robot_started ?r) + ) + :effect (and + (pipeline_found ?p) + ) + ) + + (:action inspect_pipeline + :parameters (?a - action ?p - pipeline ?r - robot ?fd1 ?fd2) + :precondition (and + (= ?a a_inspect_pipeline) + (exists (?f1 ?f2) + (and + (action_requires ?a ?f1 ?f2) + (Function ?f1) + (Function ?f2) + (FunctionDesign ?fd1) + (FunctionDesign ?fd2) + (solvesF ?fd1 ?f1) + (solvesF ?fd2 ?f2) + (not (inferred-Fd_realisability ?fd1 false_boolean)) + (not (inferred-Fd_realisability ?fd2 false_boolean)) + (system_in_mode ?f1 ?fd1) + (system_in_mode ?f2 ?fd2) + ) + ) + (not (inferred-f_activated f_generate_search_path)) + (robot_started ?r) + (pipeline_found ?p) + ) + :effect (and + (pipeline_inspected ?p) + ) + ) +) \ No newline at end of file diff --git a/plansys2_problem_expert/test/pddl/suave_domain2.pddl b/plansys2_problem_expert/test/pddl/suave_domain2.pddl new file mode 100644 index 000000000..534e7c959 --- /dev/null +++ b/plansys2_problem_expert/test/pddl/suave_domain2.pddl @@ -0,0 +1,724 @@ +(define (domain suave) + (:requirements + :strips + :typing + :adl + :negative-preconditions + :derived-predicates + :existential-preconditions + :disjunctive-preconditions + ) + + + (:types + pipeline + robot + numerical-object + ) + + (:constants + ERROR_string a_inspect_pipeline a_recharge_battery a_search_pipeline battery_level c_thruster_1 c_thruster_2 c_thruster_3 c_thruster_4 c_thruster_5 c_thruster_6 f_follow_pipeline f_generate_search_path f_maintain_motion false_boolean fd_all_thrusters fd_follow_pipeline fd_recover_thrusters fd_spiral_high fd_spiral_low fd_spiral_medium fd_unground generate_recharge_path obs_battery_level obs_water_visibility performance qa_inspect_efficiency_high qa_motion_efficiency_degraded qa_motion_efficiency_normal qa_performance_zero qa_search_efficiency_high qa_search_efficiency_low qa_search_efficiency_medium qa_water_visibility_high qa_water_visibility_low qa_water_visibility_medium true_boolean water_visibility - object + ) + + (:predicates + (pipeline_found ?p - pipeline) + (pipeline_inspected ?p - pipeline) + + (robot_started ?r - robot) + (robot_not_started ?r - robot) + + (Action ?x) + (Component ?x) + (Function ?x) + (FunctionDesign ?x) + (QAvalue ?x) + (QualityAttributeType ?x) + (c_status ?x ?y) + (differentFrom ?x ?y) + (f_active ?x ?y) + (fdBetterUtility ?x ?y) + (fd_realisability ?x ?y) + (fd_utility ?x ?y) + (functionGrounding ?x ?y) + (hasQAestimation ?x ?y) + (inferred-Action ?x) + (inferred-C_status ?x ?y) + (inferred-Component ?x) + (inferred-DifferentFrom ?x ?y) + (inferred-F_active ?x ?y) + (inferred-FdBetterUtility ?x ?y) + (inferred-Fd_realisability ?x ?y) + (inferred-Fd_utility ?x ?y) + (inferred-Function ?x) + (inferred-FunctionDesign ?x) + (inferred-FunctionGrounding ?x ?y) + (inferred-HasQAestimation ?x ?y) + (inferred-Inconsistent) + (inferred-IsQAtype ?x ?y) + (inferred-QAvalue ?x) + (inferred-Qa_has_value ?x ?y) + (inferred-QualityAttributeType ?x) + (inferred-RequiresC ?x ?y) + (inferred-RequiresF ?x ?y) + (inferred-SameAs ?x ?y) + (inferred-SolvesF ?x ?y) + (isQAtype ?x ?y) + (lessThan ?x ?y) + (qa_has_value ?x ?y) + (requiresC ?x ?y) + (requiresF ?x ?y) + (sameAs ?x ?y) + (solvesF ?x ?y) + (equalTo ?x ?y) + ) + + +(:derived (inferred-Action ?x) + (and + (Action ?x) + ) +) + + +(:derived (inferred-Action ?x) + (exists (?y) + (and + (inferred-RequiresF ?x ?y) + ) + ) + ) + + +(:derived (inferred-C_status ?x ?y) + (and + (c_status ?x ?y) + ) +) + + +(:derived (inferred-Component ?x) + (and + (Component ?x) + ) +) + + +(:derived (inferred-Component ?x) + (exists (?y) + (and + (inferred-C_status ?x ?y) + ) + ) + ) + + +(:derived (inferred-Component ?y) + (exists (?x) + (and + (inferred-RequiresC ?x ?y) + ) + ) + ) + + +(:derived (inferred-DifferentFrom ?x ?y) + (and + (differentFrom ?x ?y) + ) +) + + +(:derived (inferred-F_active ?f ?true_boolean) + (and + (= ?true_boolean true_boolean) + (exists (?fd) + (and + (inferred-Function ?f) + (inferred-FunctionDesign ?fd) + (not (= ?fd fd_unground)) + (inferred-SolvesF ?fd ?f) + (inferred-FunctionGrounding ?f ?fd) + ) + ) + ) + ) + + +(:derived (inferred-F_active ?x ?y) + (and + (f_active ?x ?y) + ) +) + + +(:derived (inferred-FdBetterUtility ?fd_better ?fd) + (exists (?x_better ?f ?x) + (and + (inferred-Fd_utility ?fd_better ?x_better) + (inferred-Function ?f) + (inferred-FunctionDesign ?fd) + (inferred-FunctionDesign ?fd_better) + (lessThan ?x ?x_better) + (inferred-SolvesF ?fd ?f) + (inferred-Fd_utility ?fd ?x) + (inferred-SolvesF ?fd_better ?f) + ) + ) + ) + + +(:derived (inferred-FdBetterUtility ?x ?y) + (and + (fdBetterUtility ?x ?y) + ) +) + + +(:derived (inferred-Fd_realisability ?fd ?false_boolean) + (and + (= ?false_boolean false_boolean) + (exists (?c) + (and + (inferred-RequiresC ?fd ?c) + (inferred-Component ?c) + (inferred-C_status ?c ERROR_string) + ) + ) + ) + ) + + +(:derived (inferred-Fd_realisability ?fd1 ?false_boolean) + (and + (= ?false_boolean false_boolean) + (exists (?eqa ?eqav ?mqa ?mqav) + (and + (inferred-FunctionDesign ?fd1) + (inferred-HasQAestimation ?fd1 ?eqa) + (inferred-IsQAtype ?eqa water_visibility) + (inferred-Qa_has_value ?eqa ?eqav) + (inferred-QAvalue ?mqa) + (= ?mqa obs_water_visibility) + (inferred-Qa_has_value ?mqa ?mqav) + (inferred-IsQAtype ?mqa water_visibility) + (lessThan ?mqav ?eqav) + ) + ) + ) + ) + + +(:derived (inferred-Fd_realisability ?x ?y) + (and + (fd_realisability ?x ?y) + ) +) + + +(:derived (inferred-Fd_utility ?fd ?qav) + (exists (?f ?qa) + (and + (inferred-Function f_follow_pipeline) + (inferred-FunctionDesign ?fd) + (inferred-SolvesF ?fd ?f) + (inferred-HasQAestimation ?fd ?qa) + (inferred-IsQAtype ?qa performance) + (inferred-Qa_has_value ?qa ?qav) + ) + ) + ) + + +(:derived (inferred-Fd_utility ?fd ?qav) + (exists (?f ?qa) + (and + (inferred-Function f_generate_search_path) + (inferred-FunctionDesign ?fd) + (inferred-SolvesF ?fd ?f) + (inferred-HasQAestimation ?fd ?qa) + (inferred-IsQAtype ?qa performance) + (inferred-Qa_has_value ?qa ?qav) + ) + ) + ) + + +(:derived (inferred-Fd_utility ?fd ?qav) + (exists (?f ?qa) + (and + (inferred-Function f_maintain_motion) + (inferred-FunctionDesign ?fd) + (inferred-SolvesF ?fd ?f) + (inferred-HasQAestimation ?fd ?qa) + (inferred-IsQAtype ?qa performance) + (inferred-Qa_has_value ?qa ?qav) + ) + ) + ) + + +(:derived (inferred-Fd_utility ?x ?y) + (and + (fd_utility ?x ?y) + ) +) + + +(:derived (inferred-Function ?x) + (and + (Function ?x) + ) +) + + +(:derived (inferred-Function ?x) + (exists (?y) + (and + (inferred-F_active ?x ?y) + ) + ) + ) + + +(:derived (inferred-Function ?x) + (exists (?y) + (and + (inferred-FunctionGrounding ?x ?y) + ) + ) + ) + + +(:derived (inferred-Function ?y) + (exists (?x) + (and + (inferred-RequiresF ?x ?y) + ) + ) + ) + + +(:derived (inferred-Function ?y) + (exists (?x) + (and + (inferred-SolvesF ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (and + (FunctionDesign ?x) + ) +) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-FdBetterUtility ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-Fd_realisability ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-Fd_utility ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-HasQAestimation ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-RequiresC ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-SolvesF ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?y) + (exists (?x) + (and + (inferred-FdBetterUtility ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?y) + (exists (?x) + (and + (inferred-FunctionGrounding ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionGrounding ?x ?y) + (and + (functionGrounding ?x ?y) + ) +) + + +(:derived (inferred-HasQAestimation ?x ?y) + (and + (hasQAestimation ?x ?y) + ) +) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-C_status ?x ?y) + (inferred-C_status ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-F_active ?x ?y) + (inferred-F_active ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Fd_realisability ?x ?y) + (inferred-Fd_realisability ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Fd_utility ?x ?y) + (inferred-Fd_utility ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-IsQAtype ?x ?y) + (inferred-IsQAtype ?x ?z) + (= ?y ?z) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Qa_has_value ?x ?y) + (inferred-Qa_has_value ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-SolvesF ?x ?y) + (inferred-SolvesF ?x ?z) + (= ?y ?z) + ) + ) + ) + + +(:derived (inferred-IsQAtype ?x ?y) + (and + (isQAtype ?x ?y) + ) +) + + +(:derived (inferred-QAvalue ?x) + (and + (QAvalue ?x) + ) +) + + +(:derived (inferred-QAvalue ?x) + (exists (?y) + (and + (inferred-IsQAtype ?x ?y) + ) + ) + ) + + +(:derived (inferred-QAvalue ?x) + (exists (?y) + (and + (inferred-Qa_has_value ?x ?y) + ) + ) + ) + + +(:derived (inferred-QAvalue ?y) + (exists (?x) + (and + (inferred-HasQAestimation ?x ?y) + ) + ) + ) + + +(:derived (inferred-Qa_has_value ?x ?y) + (and + (qa_has_value ?x ?y) + ) +) + + +(:derived (inferred-QualityAttributeType ?x) + (and + (QualityAttributeType ?x) + ) +) + + +(:derived (inferred-QualityAttributeType ?y) + (exists (?x) + (and + (inferred-IsQAtype ?x ?y) + ) + ) + ) + + +(:derived (inferred-RequiresC ?x ?y) + (and + (requiresC ?x ?y) + ) +) + + +(:derived (inferred-RequiresF ?x ?y) + (and + (requiresF ?x ?y) + ) +) + + +(:derived (inferred-SameAs ?x ?y) + (and + (sameAs ?x ?y) + ) +) + + +(:derived (inferred-SolvesF ?fd_unground ?f) + (and + (= ?fd_unground fd_unground) + (and + (inferred-Function ?f) + (inferred-FunctionDesign fd_unground) + ) + ) + ) + + +(:derived (inferred-SolvesF ?x ?y) + (and + (solvesF ?x ?y) + ) +) + + + + (:action start_robot + :parameters (?r - robot) + :precondition (and + (robot_not_started ?r) + ) + :effect (and + (not (robot_not_started ?r)) + (robot_started ?r) + ) + ) + + (:action reconfigure1 + :parameters (?f ?fd_goal) + :precondition (and + (Function ?f) + (inferred-SolvesF ?fd_goal ?f) + (FunctionDesign ?fd_goal) + (not (inferred-Fd_realisability ?fd_goal false_boolean)) + (not + (exists (?fd) + (and + (inferred-SolvesF ?fd ?f) + (FunctionDesign ?fd) + (functionGrounding ?f ?fd) + ) + ) + ) + ) + :effect (and + (functionGrounding ?f ?fd_goal) + ) + ) + + (:action reconfigure2 + :parameters (?f ?fd_initial ?fd_goal) + :precondition (and + (not (= ?fd_initial ?fd_goal)) + + (Function ?f) + (FunctionDesign ?fd_initial) + (functionGrounding ?f ?fd_initial) + + (inferred-SolvesF ?fd_goal ?f) + (FunctionDesign ?fd_goal) + (not (inferred-Fd_realisability ?fd_goal false_boolean)) + ) + :effect (and + (not (functionGrounding ?f ?fd_initial)) + (functionGrounding ?f ?fd_goal) + ) + ) + + (:action search_pipeline + :parameters (?p - pipeline ?r - robot) + :precondition (and + (robot_started ?r) + (exists (?a ?f1 ?f2 ?fd1 ?fd2) + (and + (inferred-Action ?a) + (= ?a a_search_pipeline) + (not (= ?f1 ?f2)) + (inferred-requiresF ?a ?f1) + (inferred-requiresF ?a ?f2) + (inferred-F_active ?f1 true_boolean) + (inferred-F_active ?f2 true_boolean) + (inferred-FunctionGrounding ?f1 ?fd1) + (inferred-FunctionGrounding ?f2 ?fd2) + (not + (exists (?fd1_b) + (and + (not (= ?fd1 ?fd1_b)) + (inferred-SolvesF ?fd1_b ?f1) + (not (inferred-Fd_realisability ?fd1_b false_boolean)) + (inferred-FdBetterUtility ?fd1_b ?fd1) + ) + ) + ) + (not + (exists (?fd2_b) + (and + (not (= ?fd2 ?fd2_b)) + (inferred-SolvesF ?fd2_b ?f2) + (not (inferred-Fd_realisability ?fd2_b false_boolean)) + (inferred-FdBetterUtility ?fd2_b ?fd2) + ) + ) + ) + ) + ) + (not (inferred-F_active f_follow_pipeline true_boolean)) + ) + :effect (and + (pipeline_found ?p) + ) + ) + + (:action inspect_pipeline + :parameters (?p - pipeline ?r - robot) + :precondition (and + (robot_started ?r) + (pipeline_found ?p) + (exists (?a ?f1 ?f2 ?fd1 ?fd2) + (and + (inferred-Action ?a) + (= ?a a_inspect_pipeline) + (not (= ?f1 ?f2)) + (inferred-requiresF ?a ?f1) + (inferred-requiresF ?a ?f2) + (inferred-F_active ?f1 true_boolean) + (inferred-F_active ?f2 true_boolean) + (inferred-FunctionGrounding ?f1 ?fd1) + (inferred-FunctionGrounding ?f2 ?fd2) + (not + (exists (?fd1_b) + (and + (not (= ?fd1 ?fd1_b)) + (inferred-SolvesF ?fd1_b ?f1) + (not (inferred-Fd_realisability ?fd1_b false_boolean)) + (inferred-FdBetterUtility ?fd1_b ?fd1) + ) + ) + ) + (not + (exists (?fd2_b) + (and + (not (= ?fd2 ?fd2_b)) + (inferred-SolvesF ?fd2_b ?f2) + (not (inferred-Fd_realisability ?fd2_b false_boolean)) + (inferred-FdBetterUtility ?fd2_b ?fd2) + ) + ) + ) + ) + ) + (not (inferred-F_active f_generate_search_path true_boolean)) + ) + :effect (and + (pipeline_inspected ?p) + ) + ) +) diff --git a/plansys2_problem_expert/test/pddl/suave_domain_extended.pddl b/plansys2_problem_expert/test/pddl/suave_domain_extended.pddl new file mode 100644 index 000000000..2c0339fd6 --- /dev/null +++ b/plansys2_problem_expert/test/pddl/suave_domain_extended.pddl @@ -0,0 +1,775 @@ +(define (domain suave_extended) + (:requirements + :strips + :typing + :adl + :negative-preconditions + :derived-predicates + :existential-preconditions + :disjunctive-preconditions + ) + + (:types + pipeline + robot + numerical-object + ) + + (:constants + bluerov - robot + 0.25_decimal 1.0_decimal - numerical-object + ERROR_string a_inspect_pipeline a_recharge_battery a_search_pipeline battery_level c_thruster_1 c_thruster_2 c_thruster_3 c_thruster_4 c_thruster_5 c_thruster_6 f_follow_pipeline f_generate_search_path f_maintain_motion false_boolean fd_all_thrusters fd_follow_pipeline fd_recover_thrusters fd_spiral_high fd_spiral_low fd_spiral_medium fd_unground generate_recharge_path normal obs_battery_level obs_water_visibility performance qa_inspect_efficiency_high qa_motion_efficiency_degraded qa_motion_efficiency_normal qa_performance_zero qa_search_efficiency_high qa_search_efficiency_low qa_search_efficiency_medium qa_water_visibility_high qa_water_visibility_low qa_water_visibility_medium true_boolean water_visibility - object + ) + + (:predicates + (pipeline_found ?p - pipeline) + (pipeline_inspected ?p - pipeline) + + (robot_started ?r - robot) + (robot_not_started ?r - robot) + + (inferred-battery_charged ?r - robot) + + (Action ?x) + (Component ?x) + (Function ?x) + (FunctionDesign ?x) + (QAvalue ?x) + (QualityAttributeType ?x) + (c_status ?x ?y) + (differentFrom ?x ?y) + (f_active ?x ?y) + (fdBetterUtility ?x ?y) + (fd_realisability ?x ?y) + (fd_utility ?x ?y) + (functionGrounding ?x ?y) + (hasQAestimation ?x ?y) + (inferred-Action ?x) + (inferred-C_status ?x ?y) + (inferred-Component ?x) + (inferred-DifferentFrom ?x ?y) + (inferred-F_active ?x ?y) + (inferred-FdBetterUtility ?x ?y) + (inferred-Fd_realisability ?x ?y) + (inferred-Fd_utility ?x ?y) + (inferred-Function ?x) + (inferred-FunctionDesign ?x) + (inferred-FunctionGrounding ?x ?y) + (inferred-HasQAestimation ?x ?y) + (inferred-Inconsistent) + (inferred-IsQAtype ?x ?y) + (inferred-QAvalue ?x) + (inferred-Qa_has_value ?x ?y) + (inferred-QualityAttributeType ?x) + (inferred-RequiresC ?x ?y) + (inferred-RequiresF ?x ?y) + (inferred-SameAs ?x ?y) + (inferred-SolvesF ?x ?y) + (isQAtype ?x ?y) + (lessThan ?x ?y) + (qa_has_value ?x ?y) + (requiresC ?x ?y) + (requiresF ?x ?y) + (sameAs ?x ?y) + (solvesF ?x ?y) + (equalTo ?x ?y) + ) + + +(:derived (inferred-Action ?x) + (and + (Action ?x) + ) +) + + +(:derived (inferred-Action ?x) + (exists (?y) + (and + (inferred-RequiresF ?x ?y) + ) + ) + ) + + +(:derived (inferred-C_status ?x ?y) + (and + (c_status ?x ?y) + ) +) + + +(:derived (inferred-Component ?x) + (and + (Component ?x) + ) +) + + +(:derived (inferred-Component ?x) + (exists (?y) + (and + (inferred-C_status ?x ?y) + ) + ) + ) + + +(:derived (inferred-Component ?y) + (exists (?x) + (and + (inferred-RequiresC ?x ?y) + ) + ) + ) + + +(:derived (inferred-DifferentFrom ?x ?y) + (and + (differentFrom ?x ?y) + ) +) + + +(:derived (inferred-F_active ?f ?true_boolean) + (and + (= ?true_boolean true_boolean) + (exists (?fd) + (and + (inferred-Function ?f) + (inferred-FunctionDesign ?fd) + (not (= ?fd fd_unground)) + (inferred-SolvesF ?fd ?f) + (inferred-FunctionGrounding ?f ?fd) + ) + ) + ) + ) + + +(:derived (inferred-F_active ?x ?y) + (and + (f_active ?x ?y) + ) +) + + +(:derived (inferred-FdBetterUtility ?fd_better ?fd) + (exists (?x_better ?f ?x) + (and + (inferred-Fd_utility ?fd_better ?x_better) + (inferred-Function ?f) + (inferred-FunctionDesign ?fd) + (inferred-FunctionDesign ?fd_better) + (lessThan ?x ?x_better) + (inferred-SolvesF ?fd ?f) + (inferred-Fd_utility ?fd ?x) + (inferred-SolvesF ?fd_better ?f) + ) + ) + ) + + +(:derived (inferred-FdBetterUtility ?x ?y) + (and + (fdBetterUtility ?x ?y) + ) +) + + +(:derived (inferred-Fd_realisability ?fd ?false_boolean) + (and + (= ?false_boolean false_boolean) + (exists (?c) + (and + (inferred-RequiresC ?fd ?c) + (inferred-Component ?c) + (inferred-C_status ?c ERROR_string) + ) + ) + ) + ) + + +(:derived (inferred-Fd_realisability ?fd1 ?false_boolean) + (and + (= ?false_boolean false_boolean) + (exists (?eqa ?eqav ?mqa ?mqav) + (and + (inferred-FunctionDesign ?fd1) + (inferred-HasQAestimation ?fd1 ?eqa) + (inferred-IsQAtype ?eqa water_visibility) + (inferred-Qa_has_value ?eqa ?eqav) + (inferred-QAvalue ?mqa) + (= ?mqa obs_water_visibility) + (inferred-Qa_has_value ?mqa ?mqav) + (inferred-IsQAtype ?mqa water_visibility) + (lessThan ?mqav ?eqav) + ) + ) + ) + ) + + +(:derived (inferred-Fd_realisability ?x ?y) + (and + (fd_realisability ?x ?y) + ) +) + + +(:derived (inferred-Fd_utility ?fd ?qav) + (exists (?f ?qa) + (and + (inferred-Function f_follow_pipeline) + (inferred-FunctionDesign ?fd) + (inferred-SolvesF ?fd ?f) + (inferred-HasQAestimation ?fd ?qa) + (inferred-IsQAtype ?qa performance) + (inferred-Qa_has_value ?qa ?qav) + ) + ) + ) + + +(:derived (inferred-Fd_utility ?fd ?qav) + (exists (?f ?qa) + (and + (inferred-Function f_generate_search_path) + (inferred-FunctionDesign ?fd) + (inferred-SolvesF ?fd ?f) + (inferred-HasQAestimation ?fd ?qa) + (inferred-IsQAtype ?qa performance) + (inferred-Qa_has_value ?qa ?qav) + ) + ) + ) + + +(:derived (inferred-Fd_utility ?fd ?qav) + (exists (?f ?qa) + (and + (inferred-Function f_maintain_motion) + (inferred-FunctionDesign ?fd) + (inferred-SolvesF ?fd ?f) + (inferred-HasQAestimation ?fd ?qa) + (inferred-IsQAtype ?qa performance) + (inferred-Qa_has_value ?qa ?qav) + ) + ) + ) + + +(:derived (inferred-Fd_utility ?x ?y) + (and + (fd_utility ?x ?y) + ) +) + + +(:derived (inferred-Function ?x) + (and + (Function ?x) + ) +) + + +(:derived (inferred-Function ?x) + (exists (?y) + (and + (inferred-F_active ?x ?y) + ) + ) + ) + + +(:derived (inferred-Function ?x) + (exists (?y) + (and + (inferred-FunctionGrounding ?x ?y) + ) + ) + ) + + +(:derived (inferred-Function ?y) + (exists (?x) + (and + (inferred-RequiresF ?x ?y) + ) + ) + ) + + +(:derived (inferred-Function ?y) + (exists (?x) + (and + (inferred-SolvesF ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (and + (FunctionDesign ?x) + ) +) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-FdBetterUtility ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-Fd_realisability ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-Fd_utility ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-HasQAestimation ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-RequiresC ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?x) + (exists (?y) + (and + (inferred-SolvesF ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?y) + (exists (?x) + (and + (inferred-FdBetterUtility ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionDesign ?y) + (exists (?x) + (and + (inferred-FunctionGrounding ?x ?y) + ) + ) + ) + + +(:derived (inferred-FunctionGrounding ?x ?y) + (and + (functionGrounding ?x ?y) + ) +) + + +(:derived (inferred-HasQAestimation ?x ?y) + (and + (hasQAestimation ?x ?y) + ) +) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-C_status ?x ?y) + (inferred-C_status ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-F_active ?x ?y) + (inferred-F_active ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Fd_realisability ?x ?y) + (inferred-Fd_realisability ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Fd_utility ?x ?y) + (inferred-Fd_utility ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-IsQAtype ?x ?y) + (inferred-IsQAtype ?x ?z) + (= ?y ?z) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-Qa_has_value ?x ?y) + (inferred-Qa_has_value ?x ?z) + (not (= ?y ?z)) + ) + ) + ) + + +(:derived (inferred-Inconsistent ) + (exists (?x ?y ?z) + (and + (inferred-SolvesF ?x ?y) + (inferred-SolvesF ?x ?z) + (= ?y ?z) + ) + ) + ) + + +(:derived (inferred-IsQAtype ?x ?y) + (and + (isQAtype ?x ?y) + ) +) + + +(:derived (inferred-QAvalue ?x) + (and + (QAvalue ?x) + ) +) + + +(:derived (inferred-QAvalue ?x) + (exists (?y) + (and + (inferred-IsQAtype ?x ?y) + ) + ) + ) + + +(:derived (inferred-QAvalue ?x) + (exists (?y) + (and + (inferred-Qa_has_value ?x ?y) + ) + ) + ) + + +(:derived (inferred-QAvalue ?y) + (exists (?x) + (and + (inferred-HasQAestimation ?x ?y) + ) + ) + ) + + +(:derived (inferred-Qa_has_value ?x ?y) + (and + (qa_has_value ?x ?y) + ) +) + + +(:derived (inferred-QualityAttributeType ?x) + (and + (QualityAttributeType ?x) + ) +) + + +(:derived (inferred-QualityAttributeType ?y) + (exists (?x) + (and + (inferred-IsQAtype ?x ?y) + ) + ) + ) + + +(:derived (inferred-RequiresC ?x ?y) + (and + (requiresC ?x ?y) + ) +) + + +(:derived (inferred-RequiresF ?x ?y) + (and + (requiresF ?x ?y) + ) +) + + +(:derived (inferred-SameAs ?x ?y) + (and + (sameAs ?x ?y) + ) +) + + +(:derived (inferred-SolvesF ?fd_unground ?f) + (and + (= ?fd_unground fd_unground) + (and + (inferred-Function ?f) + (inferred-FunctionDesign fd_unground) + ) + ) + ) + + +(:derived (inferred-SolvesF ?x ?y) + (and + (solvesF ?x ?y) + ) +) + + (:derived (inferred-battery_charged ?r - robot) + (and + (= ?r bluerov) + (exists (?mqa ?mqav) + (and + (= ?mqa obs_battery_level) + (inferred-Qa_has_value ?mqa ?mqav) + (inferred-IsQAtype ?mqa battery_level) + (lessThan 0.25_decimal ?mqav) + ) + ) + ) + ) + + (:action start_robot + :parameters (?r - robot) + :precondition (and + (robot_not_started ?r) + ) + :effect (and + (not (robot_not_started ?r)) + (robot_started ?r) + ) + ) + + (:action reconfigure1 + :parameters (?f ?fd_goal) + :precondition (and + (Function ?f) + (inferred-SolvesF ?fd_goal ?f) + (FunctionDesign ?fd_goal) + (not (inferred-Fd_realisability ?fd_goal false_boolean)) + (not + (exists (?fd) + (and + (inferred-SolvesF ?fd ?f) + (FunctionDesign ?fd) + (functionGrounding ?f ?fd) + ) + ) + ) + ) + :effect (and + (functionGrounding ?f ?fd_goal) + ) + ) + + (:action reconfigure2 + :parameters (?f ?fd_initial ?fd_goal) + :precondition (and + (not (= ?fd_initial ?fd_goal)) + + (Function ?f) + (FunctionDesign ?fd_initial) + (functionGrounding ?f ?fd_initial) + + (inferred-SolvesF ?fd_goal ?f) + (FunctionDesign ?fd_goal) + (not (inferred-Fd_realisability ?fd_goal false_boolean)) + ) + :effect (and + (not (functionGrounding ?f ?fd_initial)) + (functionGrounding ?f ?fd_goal) + ) + ) + + (:action search_pipeline + :parameters (?p - pipeline ?r - robot) + :precondition (and + (robot_started ?r) + (inferred-battery_charged ?r) + (exists (?a ?f1 ?f2 ?fd1 ?fd2) + (and + (inferred-Action ?a) + (= ?a a_search_pipeline) + (not (= ?f1 ?f2)) + (inferred-requiresF ?a ?f1) + (inferred-requiresF ?a ?f2) + (inferred-F_active ?f1 true_boolean) + (inferred-F_active ?f2 true_boolean) + (inferred-FunctionGrounding ?f1 ?fd1) + (inferred-FunctionGrounding ?f2 ?fd2) + (not (inferred-Fd_realisability ?fd1 false_boolean)) + (not (inferred-Fd_realisability ?fd2 false_boolean)) + (not + (exists (?fd1_b) + (and + (not (= ?fd1 ?fd1_b)) + (inferred-SolvesF ?fd1_b ?f1) + (not (inferred-Fd_realisability ?fd1_b false_boolean)) + (inferred-FdBetterUtility ?fd1_b ?fd1) + ) + ) + ) + (not + (exists (?fd2_b) + (and + (not (= ?fd2 ?fd2_b)) + (inferred-SolvesF ?fd2_b ?f2) + (not (inferred-Fd_realisability ?fd2_b false_boolean)) + (inferred-FdBetterUtility ?fd2_b ?fd2) + ) + ) + ) + ) + ) + (not (inferred-F_active f_follow_pipeline true_boolean)) + (not (inferred-F_active generate_recharge_path true_boolean)) + ) + :effect (and + (pipeline_found ?p) + ) + ) + + (:action inspect_pipeline + :parameters (?p - pipeline ?r - robot) + :precondition (and + (robot_started ?r) + (pipeline_found ?p) + (inferred-battery_charged ?r) + (exists (?a ?f1 ?f2 ?fd1 ?fd2) + (and + (inferred-Action ?a) + (= ?a a_inspect_pipeline) + (not (= ?f1 ?f2)) + (inferred-requiresF ?a ?f1) + (inferred-requiresF ?a ?f2) + (inferred-F_active ?f1 true_boolean) + (inferred-F_active ?f2 true_boolean) + (inferred-FunctionGrounding ?f1 ?fd1) + (inferred-FunctionGrounding ?f2 ?fd2) + (not (inferred-Fd_realisability ?fd1 false_boolean)) + (not (inferred-Fd_realisability ?fd2 false_boolean)) + (not + (exists (?fd1_b) + (and + (not (= ?fd1 ?fd1_b)) + (inferred-SolvesF ?fd1_b ?f1) + (not (inferred-Fd_realisability ?fd1_b false_boolean)) + (inferred-FdBetterUtility ?fd1_b ?fd1) + ) + ) + ) + (not + (exists (?fd2_b) + (and + (not (= ?fd2 ?fd2_b)) + (inferred-SolvesF ?fd2_b ?f2) + (not (inferred-Fd_realisability ?fd2_b false_boolean)) + (inferred-FdBetterUtility ?fd2_b ?fd2) + ) + ) + ) + ) + ) + (not (inferred-F_active f_generate_search_path true_boolean)) + (not (inferred-F_active generate_recharge_path true_boolean)) + ) + :effect (and + (pipeline_inspected ?p) + ) + ) + + (:action recharge_battery + :parameters (?r - robot) + :precondition (and + (robot_started ?r) + (exists (?a ?f1 ?f2 ?fd1 ?fd2) + (and + (inferred-Action ?a) + (= ?a a_recharge_battery) + (not (= ?f1 ?f2)) + (inferred-requiresF ?a ?f1) + (inferred-requiresF ?a ?f2) + (inferred-F_active ?f1 true_boolean) + (inferred-F_active ?f2 true_boolean) + (inferred-FunctionGrounding ?f1 ?fd1) + (inferred-FunctionGrounding ?f2 ?fd2) + (not (inferred-Fd_realisability ?fd1 false_boolean)) + (not (inferred-Fd_realisability ?fd2 false_boolean)) + ) + ) + (not (inferred-F_active f_generate_search_path true_boolean)) + (not (inferred-F_active f_follow_pipeline true_boolean)) + ) + :effect (and + (qa_has_value obs_battery_level 1.0_decimal) + ) + ) + +) \ No newline at end of file diff --git a/plansys2_problem_expert/test/pddl/suave_problem.pddl b/plansys2_problem_expert/test/pddl/suave_problem.pddl new file mode 100644 index 000000000..cb2fabc8b --- /dev/null +++ b/plansys2_problem_expert/test/pddl/suave_problem.pddl @@ -0,0 +1,99 @@ +(define (problem example-auv) + (:domain suave) + + (:objects + pipeline - pipeline + bluerov - robot + qa_search_efficiency_low 0.25_decimal qa_water_visibility_high 3.25_decimal qa_water_visibility_low 1.25_decimal <_string qa_search_efficiency_medium 0.5_decimal qa_motion_efficiency_degraded 0.75_decimal battery_level qa_inspect_efficiency_high 1.0_decimal qa_search_efficiency_high qa_motion_efficiency_normal qa_water_visibility_medium 2.25_decimal fd_spiral_high fd_follow_pipeline fd_spiral_medium fd_all_thrusters c_thruster_2 performance c_thruster_4 fd_spiral_low fd_recover_thrusters obs_water_visibility c_thruster_1 c_thruster_3 c_thruster_5 c_thruster_6 safety energy - object +) + + (:init + (c_status c_thruster_4 false_string) + + (action_requires a_search_pipeline f_generate_search_path f_maintain_motion) + (action_requires a_inspect_pipeline f_follow_pipeline f_maintain_motion) + (system_in_mode f_maintain_motion fd_unground) + (system_in_mode f_follow_pipeline fd_unground) + (system_in_mode f_generate_search_path fd_unground) + + (hasValue qa_search_efficiency_low 0.25_decimal) + (hasValue qa_water_visibility_high 3.25_decimal) + (hasValue qa_water_visibility_low 1.25_decimal) + (qa_comparison_operator water_visibility <_string) + (hasValue qa_search_efficiency_medium 0.5_decimal) + (hasValue qa_motion_efficiency_degraded 0.75_decimal) + (qa_comparison_operator battery_level <_string) + (hasValue qa_inspect_efficiency_high 1.0_decimal) + (hasValue qa_search_efficiency_high 1.0_decimal) + (hasValue qa_motion_efficiency_normal 1.0_decimal) + (hasValue qa_water_visibility_medium 2.25_decimal) + (solvesF fd_spiral_high f_generate_search_path) + (hasQAestimation fd_follow_pipeline qa_inspect_efficiency_high) + (isQAtype qa_water_visibility_medium water_visibility) + (solvesF fd_spiral_medium f_generate_search_path) + (requiresC fd_all_thrusters c_thruster_2) + (isQAtype qa_search_efficiency_low performance) + (requiresC fd_all_thrusters c_thruster_4) + (hasQAestimation fd_spiral_high qa_search_efficiency_high) + (isQAtype qa_water_visibility_high water_visibility) + (hasQAestimation fd_spiral_low qa_search_efficiency_low) + (isQAtype qa_motion_efficiency_degraded performance) + (hasQAestimation fd_spiral_medium qa_water_visibility_medium) + (hasQAestimation fd_spiral_medium qa_search_efficiency_medium) + (isQAtype qa_water_visibility_low water_visibility) + (solvesF fd_recover_thrusters f_maintain_motion) + (isQAtype obs_water_visibility water_visibility) + (requiresC fd_all_thrusters c_thruster_1) + (solvesF fd_follow_pipeline f_follow_pipeline) + (isQAtype qa_inspect_efficiency_high performance) + (solvesF fd_all_thrusters f_maintain_motion) + (isQAtype qa_search_efficiency_medium performance) + (isQAtype qa_search_efficiency_high performance) + (hasQAestimation fd_spiral_high qa_water_visibility_high) + (solvesF fd_spiral_low f_generate_search_path) + (hasQAestimation fd_spiral_low qa_water_visibility_low) + (isQAtype qa_motion_efficiency_normal performance) + (hasQAestimation fd_recover_thrusters qa_motion_efficiency_degraded) + (requiresC fd_all_thrusters c_thruster_3) + (hasQAestimation fd_all_thrusters qa_motion_efficiency_normal) + (requiresC fd_all_thrusters c_thruster_5) + (requiresC fd_all_thrusters c_thruster_6) + (QAvalue qa_water_visibility_high) + (FunctionDesign fd_follow_pipeline) + (QAvalue obs_water_visibility) + (QAvalue qa_search_efficiency_medium) + (Component c_thruster_6) + (FunctionDesign fd_all_thrusters) + (QAvalue qa_water_visibility_low) + (Component c_thruster_4) + (QualityAttributeType water_visibility) + (QAvalue qa_inspect_efficiency_high) + (QualityAttributeType battery_level) + (FunctionDesign fd_spiral_low) + (Component c_thruster_3) + (QAvalue qa_motion_efficiency_degraded) + (QAvalue qa_water_visibility_medium) + (QAvalue qa_search_efficiency_high) + (Function f_maintain_motion) + (Function f_follow_pipeline) + (FunctionDesign fd_recover_thrusters) + (Function f_generate_search_path) + (FunctionDesign fd_spiral_medium) + (Component c_thruster_2) + (FunctionDesign fd_spiral_high) + (QualityAttributeType safety) + (QualityAttributeType energy) + (QAvalue qa_motion_efficiency_normal) + (Component c_thruster_5) + (QAvalue qa_search_efficiency_low) + (QualityAttributeType performance) + (Component c_thruster_1) + (FunctionDesign fd_unground) + ) + + (:goal (and + (robot_started bluerov) + (pipeline_found pipeline) + (pipeline_inspected pipeline) + )) +) diff --git a/plansys2_problem_expert/test/pddl/suave_problem2.pddl b/plansys2_problem_expert/test/pddl/suave_problem2.pddl new file mode 100644 index 000000000..47892531a --- /dev/null +++ b/plansys2_problem_expert/test/pddl/suave_problem2.pddl @@ -0,0 +1,138 @@ +(define (problem example-auv) + (:domain suave) + + (:objects + pipeline - pipeline + bluerov - robot + 0.0_decimal 0.25_decimal 0.5_decimal 0.75_decimal 1.0_decimal 1.25_decimal 2.25_decimal 3.25_decimal - numerical-object +) + + (:init + ;(hasValue obs_water_visibility 2.5_decimal) + (robot_not_started bluerov) + + (equalTo 0.0_decimal 0.0_decimal) + (equalTo 0.25_decimal 0.25_decimal) + (equalTo 0.5_decimal 0.5_decimal) + (equalTo 0.75_decimal 0.75_decimal) + (equalTo 1.0_decimal 1.0_decimal) + (equalTo 1.25_decimal 1.25_decimal) + (equalTo 2.25_decimal 2.25_decimal) + (equalTo 3.25_decimal 3.25_decimal) + (lessThan 0.0_decimal 0.25_decimal) + (lessThan 0.0_decimal 0.5_decimal) + (lessThan 0.0_decimal 0.75_decimal) + (lessThan 0.0_decimal 1.0_decimal) + (lessThan 0.0_decimal 1.25_decimal) + (lessThan 0.0_decimal 2.25_decimal) + (lessThan 0.0_decimal 3.25_decimal) + (lessThan 0.25_decimal 0.5_decimal) + (lessThan 0.25_decimal 0.75_decimal) + (lessThan 0.25_decimal 1.0_decimal) + (lessThan 0.25_decimal 1.25_decimal) + (lessThan 0.25_decimal 2.25_decimal) + (lessThan 0.25_decimal 3.25_decimal) + (lessThan 0.5_decimal 0.75_decimal) + (lessThan 0.5_decimal 1.0_decimal) + (lessThan 0.5_decimal 1.25_decimal) + (lessThan 0.5_decimal 2.25_decimal) + (lessThan 0.5_decimal 3.25_decimal) + (lessThan 0.75_decimal 1.0_decimal) + (lessThan 0.75_decimal 1.25_decimal) + (lessThan 0.75_decimal 2.25_decimal) + (lessThan 0.75_decimal 3.25_decimal) + (lessThan 1.0_decimal 1.25_decimal) + (lessThan 1.0_decimal 2.25_decimal) + (lessThan 1.0_decimal 3.25_decimal) + (lessThan 1.25_decimal 2.25_decimal) + (lessThan 1.25_decimal 3.25_decimal) + (lessThan 2.25_decimal 3.25_decimal) + + (Action a_inspect_pipeline) + (Action a_search_pipeline) + (Component c_thruster_1) + (Component c_thruster_2) + (Component c_thruster_3) + (Component c_thruster_4) + (Component c_thruster_5) + (Component c_thruster_6) + (Function f_follow_pipeline) + (Function f_generate_search_path) + (Function f_maintain_motion) + (FunctionDesign fd_all_thrusters) + (FunctionDesign fd_follow_pipeline) + (FunctionDesign fd_recover_thrusters) + (FunctionDesign fd_spiral_high) + (FunctionDesign fd_spiral_low) + (FunctionDesign fd_spiral_medium) + (FunctionDesign fd_unground) + (QAvalue obs_water_visibility) + (QAvalue qa_inspect_efficiency_high) + (QAvalue qa_motion_efficiency_degraded) + (QAvalue qa_motion_efficiency_normal) + (QAvalue qa_performance_zero) + (QAvalue qa_search_efficiency_high) + (QAvalue qa_search_efficiency_low) + (QAvalue qa_search_efficiency_medium) + (QAvalue qa_water_visibility_high) + (QAvalue qa_water_visibility_low) + (QAvalue qa_water_visibility_medium) + (QualityAttributeType performance) + (QualityAttributeType water_visibility) + (hasQAestimation fd_all_thrusters qa_motion_efficiency_normal) + (hasQAestimation fd_follow_pipeline qa_inspect_efficiency_high) + (hasQAestimation fd_recover_thrusters qa_motion_efficiency_degraded) + (hasQAestimation fd_spiral_high qa_search_efficiency_high) + (hasQAestimation fd_spiral_high qa_water_visibility_high) + (hasQAestimation fd_spiral_low qa_search_efficiency_low) + (hasQAestimation fd_spiral_low qa_water_visibility_low) + (hasQAestimation fd_spiral_medium qa_search_efficiency_medium) + (hasQAestimation fd_spiral_medium qa_water_visibility_medium) + (hasQAestimation fd_unground qa_performance_zero) + (isQAtype obs_water_visibility water_visibility) + (isQAtype qa_inspect_efficiency_high performance) + (isQAtype qa_motion_efficiency_degraded performance) + (isQAtype qa_motion_efficiency_normal performance) + (isQAtype qa_performance_zero performance) + (isQAtype qa_search_efficiency_high performance) + (isQAtype qa_search_efficiency_low performance) + (isQAtype qa_search_efficiency_medium performance) + (isQAtype qa_water_visibility_high water_visibility) + (isQAtype qa_water_visibility_low water_visibility) + (isQAtype qa_water_visibility_medium water_visibility) + (qa_has_value qa_inspect_efficiency_high 1.0_decimal) + (qa_has_value qa_motion_efficiency_degraded 0.75_decimal) + (qa_has_value qa_motion_efficiency_normal 1.0_decimal) + (qa_has_value qa_performance_zero 0.0_decimal) + (qa_has_value qa_search_efficiency_high 1.0_decimal) + (qa_has_value qa_search_efficiency_low 0.25_decimal) + (qa_has_value qa_search_efficiency_medium 0.5_decimal) + (qa_has_value qa_water_visibility_high 3.25_decimal) + (qa_has_value qa_water_visibility_low 1.25_decimal) + (qa_has_value qa_water_visibility_medium 2.25_decimal) + (requiresC fd_all_thrusters c_thruster_1) + (requiresC fd_all_thrusters c_thruster_2) + (requiresC fd_all_thrusters c_thruster_3) + (requiresC fd_all_thrusters c_thruster_4) + (requiresC fd_all_thrusters c_thruster_5) + (requiresC fd_all_thrusters c_thruster_6) + (requiresF a_inspect_pipeline f_follow_pipeline) + (requiresF a_inspect_pipeline f_maintain_motion) + (requiresF a_search_pipeline f_generate_search_path) + (requiresF a_search_pipeline f_maintain_motion) + (solvesF fd_all_thrusters f_maintain_motion) + (solvesF fd_follow_pipeline f_follow_pipeline) + (solvesF fd_recover_thrusters f_maintain_motion) + (solvesF fd_spiral_high f_generate_search_path) + (solvesF fd_spiral_low f_generate_search_path) + (solvesF fd_spiral_medium f_generate_search_path) + ) + + (:goal + (and + (robot_started bluerov) + (pipeline_found pipeline) + (pipeline_inspected pipeline) + ) + ) +) diff --git a/plansys2_problem_expert/test/pddl/suave_problem_extended.pddl b/plansys2_problem_expert/test/pddl/suave_problem_extended.pddl new file mode 100644 index 000000000..6437c1499 --- /dev/null +++ b/plansys2_problem_expert/test/pddl/suave_problem_extended.pddl @@ -0,0 +1,146 @@ +(define (problem example-auv) + (:domain suave_extended) + + (:objects + pipeline - pipeline + 0.0_decimal 0.5_decimal 0.75_decimal 1.25_decimal 2.25_decimal 3.25_decimal - numerical-object +) + + (:init + (qa_has_value obs_battery_level 0.0_decimal) + (robot_not_started bluerov) + + (equalTo 0.0_decimal 0.0_decimal) + (equalTo 0.25_decimal 0.25_decimal) + (equalTo 0.5_decimal 0.5_decimal) + (equalTo 0.75_decimal 0.75_decimal) + (equalTo 1.0_decimal 1.0_decimal) + (equalTo 1.25_decimal 1.25_decimal) + (equalTo 2.25_decimal 2.25_decimal) + (equalTo 3.25_decimal 3.25_decimal) + (lessThan 0.0_decimal 0.25_decimal) + (lessThan 0.0_decimal 0.5_decimal) + (lessThan 0.0_decimal 0.75_decimal) + (lessThan 0.0_decimal 1.0_decimal) + (lessThan 0.0_decimal 1.25_decimal) + (lessThan 0.0_decimal 2.25_decimal) + (lessThan 0.0_decimal 3.25_decimal) + (lessThan 0.25_decimal 0.5_decimal) + (lessThan 0.25_decimal 0.75_decimal) + (lessThan 0.25_decimal 1.0_decimal) + (lessThan 0.25_decimal 1.25_decimal) + (lessThan 0.25_decimal 2.25_decimal) + (lessThan 0.25_decimal 3.25_decimal) + (lessThan 0.5_decimal 0.75_decimal) + (lessThan 0.5_decimal 1.0_decimal) + (lessThan 0.5_decimal 1.25_decimal) + (lessThan 0.5_decimal 2.25_decimal) + (lessThan 0.5_decimal 3.25_decimal) + (lessThan 0.75_decimal 1.0_decimal) + (lessThan 0.75_decimal 1.25_decimal) + (lessThan 0.75_decimal 2.25_decimal) + (lessThan 0.75_decimal 3.25_decimal) + (lessThan 1.0_decimal 1.25_decimal) + (lessThan 1.0_decimal 2.25_decimal) + (lessThan 1.0_decimal 3.25_decimal) + (lessThan 1.25_decimal 2.25_decimal) + (lessThan 1.25_decimal 3.25_decimal) + (lessThan 2.25_decimal 3.25_decimal) + + (Action a_inspect_pipeline) + (Action a_recharge_battery) + (Action a_search_pipeline) + (Component c_thruster_1) + (Component c_thruster_2) + (Component c_thruster_3) + (Component c_thruster_4) + (Component c_thruster_5) + (Component c_thruster_6) + (Function f_follow_pipeline) + (Function f_generate_search_path) + (Function f_maintain_motion) + (Function generate_recharge_path) + (FunctionDesign fd_all_thrusters) + (FunctionDesign fd_follow_pipeline) + (FunctionDesign fd_recover_thrusters) + (FunctionDesign fd_spiral_high) + (FunctionDesign fd_spiral_low) + (FunctionDesign fd_spiral_medium) + (FunctionDesign fd_unground) + (FunctionDesign normal) + (QAvalue obs_battery_level) + (QAvalue obs_water_visibility) + (QAvalue qa_inspect_efficiency_high) + (QAvalue qa_motion_efficiency_degraded) + (QAvalue qa_motion_efficiency_normal) + (QAvalue qa_performance_zero) + (QAvalue qa_search_efficiency_high) + (QAvalue qa_search_efficiency_low) + (QAvalue qa_search_efficiency_medium) + (QAvalue qa_water_visibility_high) + (QAvalue qa_water_visibility_low) + (QAvalue qa_water_visibility_medium) + (QualityAttributeType battery_level) + (QualityAttributeType performance) + (QualityAttributeType water_visibility) + (hasQAestimation fd_all_thrusters qa_motion_efficiency_normal) + (hasQAestimation fd_follow_pipeline qa_inspect_efficiency_high) + (hasQAestimation fd_recover_thrusters qa_motion_efficiency_degraded) + (hasQAestimation fd_spiral_high qa_search_efficiency_high) + (hasQAestimation fd_spiral_high qa_water_visibility_high) + (hasQAestimation fd_spiral_low qa_search_efficiency_low) + (hasQAestimation fd_spiral_low qa_water_visibility_low) + (hasQAestimation fd_spiral_medium qa_search_efficiency_medium) + (hasQAestimation fd_spiral_medium qa_water_visibility_medium) + (hasQAestimation fd_unground qa_performance_zero) + (isQAtype obs_battery_level battery_level) + (isQAtype obs_water_visibility water_visibility) + (isQAtype qa_inspect_efficiency_high performance) + (isQAtype qa_motion_efficiency_degraded performance) + (isQAtype qa_motion_efficiency_normal performance) + (isQAtype qa_performance_zero performance) + (isQAtype qa_search_efficiency_high performance) + (isQAtype qa_search_efficiency_low performance) + (isQAtype qa_search_efficiency_medium performance) + (isQAtype qa_water_visibility_high water_visibility) + (isQAtype qa_water_visibility_low water_visibility) + (isQAtype qa_water_visibility_medium water_visibility) + (qa_has_value qa_inspect_efficiency_high 1.0_decimal) + (qa_has_value qa_motion_efficiency_degraded 0.75_decimal) + (qa_has_value qa_motion_efficiency_normal 1.0_decimal) + (qa_has_value qa_performance_zero 0.0_decimal) + (qa_has_value qa_search_efficiency_high 1.0_decimal) + (qa_has_value qa_search_efficiency_low 0.25_decimal) + (qa_has_value qa_search_efficiency_medium 0.5_decimal) + (qa_has_value qa_water_visibility_high 3.25_decimal) + (qa_has_value qa_water_visibility_low 1.25_decimal) + (qa_has_value qa_water_visibility_medium 2.25_decimal) + (requiresC fd_all_thrusters c_thruster_1) + (requiresC fd_all_thrusters c_thruster_2) + (requiresC fd_all_thrusters c_thruster_3) + (requiresC fd_all_thrusters c_thruster_4) + (requiresC fd_all_thrusters c_thruster_5) + (requiresC fd_all_thrusters c_thruster_6) + (requiresF a_inspect_pipeline f_follow_pipeline) + (requiresF a_inspect_pipeline f_maintain_motion) + (requiresF a_recharge_battery f_maintain_motion) + (requiresF a_recharge_battery generate_recharge_path) + (requiresF a_search_pipeline f_generate_search_path) + (requiresF a_search_pipeline f_maintain_motion) + (solvesF fd_all_thrusters f_maintain_motion) + (solvesF fd_follow_pipeline f_follow_pipeline) + (solvesF fd_recover_thrusters f_maintain_motion) + (solvesF fd_spiral_high f_generate_search_path) + (solvesF fd_spiral_low f_generate_search_path) + (solvesF fd_spiral_medium f_generate_search_path) + (solvesF normal generate_recharge_path) + ) + + (:goal + (and + (robot_started bluerov) + (pipeline_found pipeline) + (pipeline_inspected pipeline) + ) + ) +) \ No newline at end of file diff --git a/plansys2_problem_expert/test/unit/CMakeLists.txt b/plansys2_problem_expert/test/unit/CMakeLists.txt index bcfb5402f..8d2095421 100644 --- a/plansys2_problem_expert/test/unit/CMakeLists.txt +++ b/plansys2_problem_expert/test/unit/CMakeLists.txt @@ -1,16 +1,16 @@ -ament_add_gtest(utils_test utils_test.cpp) +ament_add_gtest(utils_test utils_test.cpp TIMEOUT 1200) target_link_libraries(utils_test ${PROJECT_NAME} ament_index_cpp::ament_index_cpp ) -ament_add_gtest(problem_expert_test problem_expert_test.cpp) +ament_add_gtest(problem_expert_test problem_expert_test.cpp TIMEOUT 1200) target_link_libraries(problem_expert_test ${PROJECT_NAME} ament_index_cpp::ament_index_cpp ) -ament_add_gtest(problem_expert_node_test problem_expert_node_test.cpp) +ament_add_gtest(problem_expert_node_test problem_expert_node_test.cpp TIMEOUT 1200) target_link_libraries(problem_expert_node_test ${PROJECT_NAME} ament_index_cpp::ament_index_cpp diff --git a/plansys2_problem_expert/test/unit/problem_expert_node_test.cpp b/plansys2_problem_expert/test/unit/problem_expert_node_test.cpp index 26b9a807b..54128c837 100644 --- a/plansys2_problem_expert/test/unit/problem_expert_node_test.cpp +++ b/plansys2_problem_expert/test/unit/problem_expert_node_test.cpp @@ -12,25 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include -#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" - +#include "plansys2_domain_expert/DomainExpert.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "plansys2_msgs/msg/knowledge.hpp" #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_problem_expert/ProblemExpert.hpp" -#include "plansys2_domain_expert/DomainExpert.hpp" -#include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" #include "plansys2_problem_expert/ProblemExpertClient.hpp" - -#include "plansys2_msgs/msg/knowledge.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" TEST(problem_expert_node, addget_instances) { @@ -45,7 +41,6 @@ TEST(problem_expert_node, addget_instances) domain_node->set_parameter({"model_file", pkgpath + "/pddl/domain_simple.pddl"}); problem_node->set_parameter({"model_file", pkgpath + "/pddl/domain_simple.pddl"}); - domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -62,15 +57,17 @@ TEST(problem_expert_node, addget_instances) int knowledge_msg_counter = 0; auto knowledge_sub = test_node_2->create_subscription( "problem_expert/knowledge", rclcpp::QoS(100).transient_local(), - [&last_knowledge_msg, &knowledge_msg_counter] - (const plansys2_msgs::msg::Knowledge::SharedPtr msg) { + [&last_knowledge_msg, + &knowledge_msg_counter](const plansys2_msgs::msg::Knowledge::SharedPtr msg) { last_knowledge_msg = *msg; knowledge_msg_counter++; }); bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("Paco", "person"))); @@ -89,9 +86,9 @@ TEST(problem_expert_node, addget_instances) ASSERT_EQ(knowledge_msg_counter, 4u); ASSERT_EQ(last_knowledge_msg.instances.size(), 3u); - ASSERT_EQ(last_knowledge_msg.instances[0], "Paco"); + ASSERT_EQ(last_knowledge_msg.instances[0], "kitchen"); ASSERT_EQ(last_knowledge_msg.instances[1], "bedroom"); - ASSERT_EQ(last_knowledge_msg.instances[2], "kitchen"); + ASSERT_EQ(last_knowledge_msg.instances[2], "Paco"); ASSERT_EQ(last_knowledge_msg.predicates.size(), 0); ASSERT_EQ(last_knowledge_msg.goal, ""); @@ -107,31 +104,26 @@ TEST(problem_expert_node, addget_instances) ASSERT_EQ(knowledge_msg_counter, 5u); ASSERT_EQ(last_knowledge_msg.instances.size(), 4u); - ASSERT_EQ(last_knowledge_msg.instances[0], "Paco"); - ASSERT_EQ(last_knowledge_msg.instances[1], "bedroom"); - ASSERT_EQ(last_knowledge_msg.instances[2], "kitchen"); - ASSERT_EQ(last_knowledge_msg.instances[3], "r2d2"); + ASSERT_EQ(last_knowledge_msg.instances[0], "r2d2"); + ASSERT_EQ(last_knowledge_msg.instances[1], "kitchen"); + ASSERT_EQ(last_knowledge_msg.instances[2], "bedroom"); + ASSERT_EQ(last_knowledge_msg.instances[3], "Paco"); ASSERT_EQ(last_knowledge_msg.predicates.size(), 0); ASSERT_EQ(last_knowledge_msg.goal, ""); - ASSERT_EQ(problem_client->getInstances().size(), 4); - ASSERT_EQ(problem_client->getInstances()[0].name, "Paco"); - ASSERT_EQ(problem_client->getInstances()[0].type, "person"); - ASSERT_EQ(problem_client->getInstances()[1].name, "bedroom"); - ASSERT_EQ(problem_client->getInstances()[1].type, "room"); - ASSERT_EQ(problem_client->getInstances()[2].name, "kitchen"); - ASSERT_EQ(problem_client->getInstances()[2].type, "room"); - ASSERT_EQ(problem_client->getInstances()[3].name, "r2d2"); - ASSERT_EQ(problem_client->getInstances()[3].type, "robot"); + auto instances = problem_client->getInstances(); + ASSERT_EQ(instances.size(), 4); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("Paco", "person")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("bedroom", "room")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("kitchen", "room")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("r2d2", "robot")) != instances.end()); ASSERT_TRUE(problem_client->removeInstance(plansys2::Instance("Paco", "person"))); - ASSERT_EQ(problem_client->getInstances().size(), 3); - ASSERT_EQ(problem_client->getInstances()[0].name, "bedroom"); - ASSERT_EQ(problem_client->getInstances()[0].type, "room"); - ASSERT_EQ(problem_client->getInstances()[1].name, "kitchen"); - ASSERT_EQ(problem_client->getInstances()[1].type, "room"); - ASSERT_EQ(problem_client->getInstances()[2].name, "r2d2"); - ASSERT_EQ(problem_client->getInstances()[2].type, "robot"); + instances = problem_client->getInstances(); + ASSERT_EQ(instances.size(), 3); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("bedroom", "room")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("kitchen", "room")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("r2d2", "robot")) != instances.end()); { rclcpp::Rate rate(10); @@ -143,9 +135,9 @@ TEST(problem_expert_node, addget_instances) ASSERT_EQ(knowledge_msg_counter, 6u); ASSERT_EQ(last_knowledge_msg.instances.size(), 3u); - ASSERT_EQ(last_knowledge_msg.instances[0], "bedroom"); + ASSERT_EQ(last_knowledge_msg.instances[0], "r2d2"); ASSERT_EQ(last_knowledge_msg.instances[1], "kitchen"); - ASSERT_EQ(last_knowledge_msg.instances[2], "r2d2"); + ASSERT_EQ(last_knowledge_msg.instances[2], "bedroom"); ASSERT_EQ(last_knowledge_msg.predicates.size(), 0); ASSERT_EQ(last_knowledge_msg.goal, ""); @@ -175,12 +167,26 @@ TEST(problem_expert_node, addget_instances) ASSERT_EQ(knowledge_msg_counter, 8u); ASSERT_EQ(last_knowledge_msg.instances.size(), 3u); - ASSERT_EQ(last_knowledge_msg.instances[0], "bedroom"); - ASSERT_EQ(last_knowledge_msg.instances[1], "kitchen"); - ASSERT_EQ(last_knowledge_msg.instances[2], "r2d2"); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "r2d2") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "kitchen") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "bedroom") != + last_knowledge_msg.instances.end()); ASSERT_EQ(last_knowledge_msg.predicates.size(), 2u); - ASSERT_EQ(last_knowledge_msg.predicates[0], "(robot_at r2d2 bedroom)"); - ASSERT_EQ(last_knowledge_msg.predicates[1], "(robot_at r2d2 kitchen)"); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(robot_at r2d2 bedroom)") != last_knowledge_msg.predicates.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(robot_at r2d2 kitchen)") != last_knowledge_msg.predicates.end()); ASSERT_EQ(last_knowledge_msg.goal, ""); ASSERT_TRUE(problem_client->setGoal(plansys2::Goal("(and (robot_at r2d2 kitchen))"))); @@ -194,12 +200,26 @@ TEST(problem_expert_node, addget_instances) } ASSERT_EQ(knowledge_msg_counter, 9u); - ASSERT_EQ(last_knowledge_msg.instances[0], "bedroom"); - ASSERT_EQ(last_knowledge_msg.instances[1], "kitchen"); - ASSERT_EQ(last_knowledge_msg.instances[2], "r2d2"); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "r2d2") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "kitchen") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "bedroom") != + last_knowledge_msg.instances.end()); ASSERT_EQ(last_knowledge_msg.predicates.size(), 2u); - ASSERT_EQ(last_knowledge_msg.predicates[0], "(robot_at r2d2 bedroom)"); - ASSERT_EQ(last_knowledge_msg.predicates[1], "(robot_at r2d2 kitchen)"); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(robot_at r2d2 bedroom)") != last_knowledge_msg.predicates.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(robot_at r2d2 kitchen)") != last_knowledge_msg.predicates.end()); ASSERT_EQ(last_knowledge_msg.goal, "(and (robot_at r2d2 kitchen))"); ASSERT_EQ(problem_client->getProblem(), problem_client->getProblem(true)); @@ -340,20 +360,38 @@ TEST(problem_expert, add_assignments) ASSERT_FALSE(problem_client->removeFunction(function_3)); - ASSERT_TRUE(problem_client->removeInstance("kitchen")); } +*/ TEST(problem_expert, addget_predicates) { + auto domain_node = std::make_shared(); + auto problem_node = std::make_shared(); + auto problem_client = std::make_shared(); + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); - std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); - auto domain_expert = std::make_shared(domain_str); - plansys2::ProblemExpert problem_expert(domain_expert); + domain_node->set_parameter({"model_file", pkgpath + "/pddl/domain_simple.pddl"}); + problem_node->set_parameter({"model_file", pkgpath + "/pddl/domain_simple.pddl"}); + + domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + rclcpp::executors::MultiThreadedExecutor exe(rclcpp::ExecutorOptions(), 8); + + exe.add_node(domain_node->get_node_base_interface()); + exe.add_node(problem_node->get_node_base_interface()); + + bool finish = false; + std::thread t([&]() { + while (!finish) { + exe.spin_some(); + } + }); plansys2_msgs::msg::Node predicate_1; predicate_1.node_type = plansys2_msgs::msg::Node::PREDICATE; @@ -393,7 +431,6 @@ TEST(problem_expert, addget_predicates) ASSERT_EQ(predicate_4.parameters[1].name, "kitchen"); ASSERT_EQ(predicate_4.parameters[1].type, "room"); - plansys2_msgs::msg::Node predicate_5; predicate_5.node_type = plansys2_msgs::msg::Node::PREDICATE; predicate_5.name = "person_at"; @@ -413,7 +450,7 @@ TEST(problem_expert, addget_predicates) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("bedroom", "room"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("kitchen", "room"))); - std::vector predicates = problem_client->getPredicates(); + auto predicates = problem_client->getPredicates(); ASSERT_TRUE(predicates.empty()); ASSERT_TRUE(problem_client->addPredicate(predicate_1)); @@ -431,7 +468,7 @@ TEST(problem_expert, addget_predicates) ASSERT_FALSE(problem_client->removePredicate(predicate_5)); ASSERT_TRUE(problem_client->removePredicate(predicate_4)); - ASSERT_TRUE(problem_client->removePredicate(predicate_4)); + ASSERT_FALSE(problem_client->removePredicate(predicate_4)); predicates = problem_client->getPredicates(); ASSERT_EQ(predicates.size(), 3); @@ -441,7 +478,8 @@ TEST(problem_expert, addget_predicates) plansys2_msgs::msg::Node predicate_7; predicate_7.node_type = plansys2_msgs::msg::Node::PREDICATE; predicate_7.name = "is_teleporter_enabled"; - predicate_7.parameters.push_back(parser::pddl::fromStringParam("bathroom", "room_with_teleporter")); + predicate_7.parameters.push_back( + parser::pddl::fromStringParam("bathroom", "room_with_teleporter")); ASSERT_EQ(predicate_7.name, "is_teleporter_enabled"); ASSERT_EQ(predicate_7.parameters.size(), 1); @@ -453,7 +491,8 @@ TEST(problem_expert, addget_predicates) plansys2_msgs::msg::Node predicate_8; predicate_8.node_type = plansys2_msgs::msg::Node::PREDICATE; predicate_8.name = "is_teleporter_destination"; - predicate_8.parameters.push_back(parser::pddl::fromStringParam("bathroom", "room_with_teleporter")); + predicate_8.parameters.push_back( + parser::pddl::fromStringParam("bathroom", "room_with_teleporter")); ASSERT_EQ(predicate_8.name, "is_teleporter_destination"); ASSERT_EQ(predicate_8.parameters.size(), 1); @@ -462,51 +501,37 @@ TEST(problem_expert, addget_predicates) ASSERT_TRUE(problem_client->addPredicate(predicate_8)); - ASSERT_TRUE(problem_client->removeInstance("bathroom")); -} - -TEST(problem_expert, addget_goals) -{ - std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); - std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); - - auto domain_expert = std::make_shared(domain_str); - plansys2::ProblemExpert problem_expert(domain_expert); - - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("paco", "person"))); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("r2d2", "robot"))); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("bedroom", "room"))); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("kitchen", "room"))); + ASSERT_TRUE( + problem_client->removeInstance(plansys2::Instance("bathroom", "room_with_teleporter"))); - plansys2_msgs::msg::Tree goal; - parser::pddl::fromString(goal, "(and (robot_at r2d2 bedroom)(person_at paco kitchen))"); - ASSERT_EQ(parser::pddl::toString(goal), "(and (robot_at r2d2 bedroom)(person_at paco kitchen))"); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("linus", "person"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("hallway", "room"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("office", "room"))); - plansys2_msgs::msg::Tree goal2; - parser::pddl::fromString(goal2, "(and (robot_at r2d2 bedroom)(not(person_at paco kitchen)))"); - ASSERT_EQ(parser::pddl::toString(goal2), "(and (robot_at r2d2 bedroom)(not (person_at paco kitchen)))"); + std::vector predicates1; + predicates1.push_back(parser::pddl::fromStringPredicate("(person_at linus hallway)")); + predicates1.push_back(parser::pddl::fromStringPredicate("(person_at linus office)")); - ASSERT_TRUE(problem_client->setGoal(goal)); - ASSERT_TRUE(problem_client->setGoal(goal2)); + ASSERT_TRUE(problem_client->addPredicates(predicates1)); - ASSERT_EQ( - problem_client->getGoal().toString(), - "(and (robot_at r2d2 bedroom)(not (person_at paco kitchen)))"); + ASSERT_TRUE(problem_client->addInstance(parser::pddl::fromStringParam("stallman", "person"))); - const plansys2_msgs::msg::Tree & goal3 = problem_client->getGoal(); - ASSERT_EQ(parser::pddl::toString(goal3), "(and (robot_at r2d2 bedroom)(not (person_at paco kitchen)))"); + std::vector predicates2; + predicates2.push_back(parser::pddl::fromStringPredicate("(person_at stallman hallway)")); + predicates2.push_back(parser::pddl::fromStringPredicate("(person_at stallman office)")); - ASSERT_TRUE(problem_client->clearGoal()); - ASSERT_TRUE(problem_client->clearGoal()); + ASSERT_TRUE(problem_client->updatePredicates(predicates2, predicates1)); - ASSERT_EQ(problem_client->getGoal().toString(), ""); + ASSERT_FALSE(problem_client->existPredicate(predicates1[0])); + ASSERT_FALSE(problem_client->existPredicate(predicates1[1])); + ASSERT_TRUE(problem_client->existPredicate(predicates2[0])); + ASSERT_TRUE(problem_client->existPredicate(predicates2[1])); - ASSERT_TRUE(problem_client->setGoal(plansys2::Goal("(and (or (robot_at r2d2 bedroom)(robot_at r2d2 kitchen))(not(person_at paco kitchen)))"))); + finish = true; + t.join(); } +/* TEST(problem_expert, get_probem) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); @@ -563,7 +588,6 @@ TEST(problem_expert, get_probem) std::string("and\n\t\t( robot_at r2d2 bedroom )\n\t\t( person_at paco kitchen )\n\t)\n)\n)\n")); } - TEST(problem_expert, set_goal) { std::string expresion = std::string("(and (patrolled ro1) (patrolled ro2) (patrolled ro3))"); @@ -587,7 +611,6 @@ TEST(problem_expert_node, addget_goal_is_satisfied) domain_node->set_parameter({"model_file", pkgpath + "/pddl/domain_simple.pddl"}); problem_node->set_parameter({"model_file", pkgpath + "/pddl/domain_simple.pddl"}); - domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -604,15 +627,17 @@ TEST(problem_expert_node, addget_goal_is_satisfied) int knowledge_msg_counter = 0; auto knowledge_sub = test_node_2->create_subscription( "problem_expert/knowledge", rclcpp::QoS(100).transient_local(), - [&last_knowledge_msg, &knowledge_msg_counter] - (const plansys2_msgs::msg::Knowledge::SharedPtr msg) { + [&last_knowledge_msg, + &knowledge_msg_counter](const plansys2_msgs::msg::Knowledge::SharedPtr msg) { last_knowledge_msg = *msg; knowledge_msg_counter++; }); bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("leia", "robot"))); @@ -631,18 +656,16 @@ TEST(problem_expert_node, addget_goal_is_satisfied) ASSERT_EQ(knowledge_msg_counter, 5u); ASSERT_EQ(last_knowledge_msg.instances.size(), 5u); - ASSERT_EQ(last_knowledge_msg.instances[0], "leia"); - ASSERT_EQ(last_knowledge_msg.instances[1], "jack"); + ASSERT_EQ(last_knowledge_msg.instances[0], "m1"); + ASSERT_EQ(last_knowledge_msg.instances[1], "kitchen"); ASSERT_EQ(last_knowledge_msg.instances[2], "bedroom"); - ASSERT_EQ(last_knowledge_msg.instances[3], "kitchen"); - ASSERT_EQ(last_knowledge_msg.instances[4], "m1"); + ASSERT_EQ(last_knowledge_msg.instances[3], "jack"); + ASSERT_EQ(last_knowledge_msg.instances[4], "leia"); ASSERT_EQ(last_knowledge_msg.predicates.size(), 0); ASSERT_EQ(last_knowledge_msg.goal, ""); - ASSERT_TRUE( - problem_client->addPredicate(plansys2::Predicate("(robot_at leia kitchen)"))); - ASSERT_TRUE( - problem_client->addPredicate(plansys2::Predicate("(person_at jack bedroom)"))); + ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate("(robot_at leia kitchen)"))); + ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate("(person_at jack bedroom)"))); std::string expression = "(and (robot_talk leia m1 jack))"; plansys2_msgs::msg::Tree goal; @@ -660,32 +683,70 @@ TEST(problem_expert_node, addget_goal_is_satisfied) ASSERT_EQ(knowledge_msg_counter, 8u); ASSERT_EQ(last_knowledge_msg.instances.size(), 5u); - ASSERT_EQ(last_knowledge_msg.instances[0], "leia"); - ASSERT_EQ(last_knowledge_msg.instances[1], "jack"); - ASSERT_EQ(last_knowledge_msg.instances[2], "bedroom"); - ASSERT_EQ(last_knowledge_msg.instances[3], "kitchen"); - ASSERT_EQ(last_knowledge_msg.instances[4], "m1"); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "m1") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "kitchen") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "bedroom") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "jack") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "leia") != + last_knowledge_msg.instances.end()); ASSERT_EQ(last_knowledge_msg.predicates.size(), 2u); - ASSERT_EQ(last_knowledge_msg.predicates[0], "(robot_at leia kitchen)"); - ASSERT_EQ(last_knowledge_msg.predicates[1], "(person_at jack bedroom)"); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(robot_at leia kitchen)") != last_knowledge_msg.predicates.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(person_at jack bedroom)") != last_knowledge_msg.predicates.end()); ASSERT_EQ(last_knowledge_msg.goal, "(and (robot_talk leia m1 jack))"); - ASSERT_TRUE( - problem_client->addPredicate(plansys2::Predicate("(robot_talk leia m1 jack)"))); + ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate("(robot_talk leia m1 jack)"))); ASSERT_TRUE(problem_client->isGoalSatisfied(goal)); ASSERT_EQ(knowledge_msg_counter, 9u); ASSERT_EQ(last_knowledge_msg.instances.size(), 5u); - ASSERT_EQ(last_knowledge_msg.instances[0], "leia"); - ASSERT_EQ(last_knowledge_msg.instances[1], "jack"); - ASSERT_EQ(last_knowledge_msg.instances[2], "bedroom"); - ASSERT_EQ(last_knowledge_msg.instances[3], "kitchen"); - ASSERT_EQ(last_knowledge_msg.instances[4], "m1"); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "m1") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "kitchen") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "bedroom") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "jack") != + last_knowledge_msg.instances.end()); + ASSERT_TRUE( + std::find(last_knowledge_msg.instances.begin(), last_knowledge_msg.instances.end(), "leia") != + last_knowledge_msg.instances.end()); ASSERT_EQ(last_knowledge_msg.predicates.size(), 3u); - ASSERT_EQ(last_knowledge_msg.predicates[0], "(robot_at leia kitchen)"); - ASSERT_EQ(last_knowledge_msg.predicates[1], "(person_at jack bedroom)"); - ASSERT_EQ(last_knowledge_msg.predicates[2], "(robot_talk leia m1 jack)"); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(robot_at leia kitchen)") != last_knowledge_msg.predicates.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(robot_talk leia m1 jack)") != last_knowledge_msg.predicates.end()); + ASSERT_TRUE( + std::find( + last_knowledge_msg.predicates.begin(), last_knowledge_msg.predicates.end(), + "(person_at jack bedroom)") != last_knowledge_msg.predicates.end()); ASSERT_EQ(last_knowledge_msg.goal, "(and (robot_talk leia m1 jack))"); finish = true; diff --git a/plansys2_problem_expert/test/unit/problem_expert_test.cpp b/plansys2_problem_expert/test/unit/problem_expert_test.cpp index 4aa428720..6fa6d17a7 100644 --- a/plansys2_problem_expert/test/unit/problem_expert_test.cpp +++ b/plansys2_problem_expert/test/unit/problem_expert_test.cpp @@ -12,28 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include -#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" - +#include "plansys2_domain_expert/DomainExpert.hpp" #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_problem_expert/ProblemExpert.hpp" -#include "plansys2_domain_expert/DomainExpert.hpp" TEST(problem_expert, addget_instances) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -41,28 +38,21 @@ TEST(problem_expert, addget_instances) ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("Paco", "person"))); ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("Paco", "person"))); ASSERT_FALSE(problem_expert.addInstance(parser::pddl::fromStringParam("Paco", "room"))); - ASSERT_FALSE( - problem_expert.addInstance( - parser::pddl::fromStringParam( - "Paco", - "SCIENTIFIC"))); + ASSERT_FALSE(problem_expert.addInstance(parser::pddl::fromStringParam("Paco", "SCIENTIFIC"))); ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("r2d2", "robot"))); ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("ur5e", "Robot"))); - ASSERT_EQ(problem_expert.getInstances().size(), 3); - ASSERT_EQ(problem_expert.getInstances()[0].name, "Paco"); - ASSERT_EQ(problem_expert.getInstances()[0].type, "person"); - ASSERT_EQ(problem_expert.getInstances()[1].name, "r2d2"); - ASSERT_EQ(problem_expert.getInstances()[1].type, "robot"); - ASSERT_EQ(problem_expert.getInstances()[2].name, "ur5e"); - ASSERT_EQ(problem_expert.getInstances()[2].type, "robot"); + auto instances = problem_expert.getInstances(); + ASSERT_EQ(instances.size(), 3); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("Paco", "person")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("r2d2", "robot")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("ur5e", "robot")) != instances.end()); ASSERT_TRUE(problem_expert.removeInstance(parser::pddl::fromStringParam("Paco", "person"))); - ASSERT_EQ(problem_expert.getInstances().size(), 2); - ASSERT_EQ(problem_expert.getInstances()[0].name, "r2d2"); - ASSERT_EQ(problem_expert.getInstances()[0].type, "robot"); - ASSERT_EQ(problem_expert.getInstances()[1].name, "ur5e"); - ASSERT_EQ(problem_expert.getInstances()[1].type, "robot"); + instances = problem_expert.getInstances(); + ASSERT_EQ(instances.size(), 2); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("r2d2", "robot")) != instances.end()); + ASSERT_TRUE(instances.find(parser::pddl::fromStringParam("ur5e", "robot")) != instances.end()); auto paco_instance = problem_expert.getInstance("Paco"); ASSERT_FALSE(paco_instance); @@ -80,19 +70,15 @@ TEST(problem_expert, add_functions) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("bedroom", "room"))); ASSERT_TRUE( - problem_expert.addInstance( - parser::pddl::fromStringParam( - "kitchen", - "room_with_teleporter"))); + problem_expert.addInstance(parser::pddl::fromStringParam("kitchen", "room_with_teleporter"))); plansys2_msgs::msg::Node function_1; function_1.node_type = plansys2_msgs::msg::Node::FUNCTION; @@ -114,10 +100,9 @@ TEST(problem_expert, add_functions) ASSERT_EQ( problem_expert.getProblem(), std::string("( define ( problem problem_1 )\n( :domain simple )\n") + - std::string("( :objects\n\tbedroom - room\n\tkitchen - room_with_teleporter\n)\n") + - std::string("( :init\n\t( = ( room_distance bedroom kitchen ) 1.2300000000 )\n)\n") + - std::string("( :goal\n\t( and\n\t))\n)\n") - ); + std::string("( :objects\n\tbedroom - room\n\tkitchen - room_with_teleporter\n)\n") + + std::string("( :init\n\t( = ( room_distance bedroom kitchen ) 1.2300000000 )\n)\n") + + std::string("( :goal\n\t( and\n\t))\n)\n")); plansys2_msgs::msg::Node function_2; function_2.node_type = plansys2_msgs::msg::Node::FUNCTION; @@ -139,10 +124,10 @@ TEST(problem_expert, add_functions) ASSERT_EQ( problem_expert.getProblem(), std::string("( define ( problem problem_1 )\n( :domain simple )\n") + - std::string("( :objects\n\tbedroom - room\n\tkitchen - room_with_teleporter\n)\n") + - std::string("( :init\n\t( = ( room_distance bedroom kitchen ) 1.2300000000 )\n") + - std::string("\t( = ( room_distance kitchen bedroom ) 2.3400000000 )\n)\n") + - std::string("( :goal\n\t( and\n\t))\n)\n")); + std::string("( :objects\n\tbedroom - room\n\tkitchen - room_with_teleporter\n)\n") + + std::string("( :init\n\t( = ( room_distance kitchen bedroom ) 2.3400000000 )\n") + + std::string("\t( = ( room_distance bedroom kitchen ) 1.2300000000 )\n)\n") + + std::string("( :goal\n\t( and\n\t))\n)\n")); function_2.value = 3.45; @@ -151,10 +136,10 @@ TEST(problem_expert, add_functions) ASSERT_EQ( problem_expert.getProblem(), std::string("( define ( problem problem_1 )\n( :domain simple )\n") + - std::string("( :objects\n\tbedroom - room\n\tkitchen - room_with_teleporter\n)\n") + - std::string("( :init\n\t( = ( room_distance bedroom kitchen ) 1.2300000000 )\n") + - std::string("\t( = ( room_distance kitchen bedroom ) 3.4500000000 )\n)\n") + - std::string("( :goal\n\t( and\n\t))\n)\n")); + std::string("( :objects\n\tbedroom - room\n\tkitchen - room_with_teleporter\n)\n") + + std::string("( :init\n\t( = ( room_distance kitchen bedroom ) 3.4500000000 )\n") + + std::string("\t( = ( room_distance bedroom kitchen ) 1.2300000000 )\n)\n") + + std::string("( :goal\n\t( and\n\t))\n)\n")); plansys2_msgs::msg::Node function_3; function_3.node_type = plansys2_msgs::msg::Node::FUNCTION; @@ -167,20 +152,16 @@ TEST(problem_expert, add_functions) ASSERT_FALSE(problem_expert.removeFunction(function_3)); - ASSERT_TRUE( - problem_expert.removeInstance( - parser::pddl::fromStringParam( - "kitchen", - "room_with_teleporter"))); + ASSERT_TRUE(problem_expert.removeInstance( + parser::pddl::fromStringParam("kitchen", "room_with_teleporter"))); } TEST(problem_expert, addget_predicates) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -223,7 +204,6 @@ TEST(problem_expert, addget_predicates) ASSERT_EQ(predicate_4.parameters[1].name, "kitchen"); ASSERT_EQ(predicate_4.parameters[1].type, "room"); - plansys2_msgs::msg::Node predicate_5; predicate_5.node_type = plansys2_msgs::msg::Node::PREDICATE; predicate_5.name = "person_at"; @@ -243,7 +223,7 @@ TEST(problem_expert, addget_predicates) ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("bedroom", "room"))); ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("kitchen", "room"))); - std::vector predicates = problem_expert.getPredicates(); + std::unordered_set predicates = problem_expert.getPredicates(); ASSERT_TRUE(predicates.empty()); ASSERT_TRUE(problem_expert.addPredicate(predicate_1)); @@ -270,24 +250,19 @@ TEST(problem_expert, addget_predicates) ASSERT_FALSE(problem_expert.removePredicate(predicate_5)); ASSERT_TRUE(problem_expert.removePredicate(predicate_4)); - ASSERT_TRUE(problem_expert.removePredicate(predicate_4)); + ASSERT_FALSE(problem_expert.removePredicate(predicate_4)); predicates = problem_expert.getPredicates(); ASSERT_EQ(predicates.size(), 3); ASSERT_TRUE( - problem_expert.addInstance( - parser::pddl::fromStringParam( - "bathroom", - "room_with_teleporter"))); + problem_expert.addInstance(parser::pddl::fromStringParam("bathroom", "room_with_teleporter"))); plansys2_msgs::msg::Node predicate_7; predicate_7.node_type = plansys2_msgs::msg::Node::PREDICATE; predicate_7.name = "is_teleporter_enabled"; predicate_7.parameters.push_back( - parser::pddl::fromStringParam( - "bathroom", - "room_with_teleporter")); + parser::pddl::fromStringParam("bathroom", "room_with_teleporter")); ASSERT_EQ(predicate_7.name, "is_teleporter_enabled"); ASSERT_EQ(predicate_7.parameters.size(), 1); @@ -300,9 +275,7 @@ TEST(problem_expert, addget_predicates) predicate_8.node_type = plansys2_msgs::msg::Node::PREDICATE; predicate_8.name = "is_teleporter_destination"; predicate_8.parameters.push_back( - parser::pddl::fromStringParam( - "bathroom", - "room_with_teleporter")); + parser::pddl::fromStringParam("bathroom", "room_with_teleporter")); ASSERT_EQ(predicate_8.name, "is_teleporter_destination"); ASSERT_EQ(predicate_8.parameters.size(), 1); @@ -311,20 +284,39 @@ TEST(problem_expert, addget_predicates) ASSERT_TRUE(problem_expert.addPredicate(predicate_8)); - ASSERT_TRUE( - problem_expert.removeInstance( - parser::pddl::fromStringParam( - "bathroom", - "room_with_teleporter"))); + ASSERT_TRUE(problem_expert.removeInstance( + parser::pddl::fromStringParam("bathroom", "room_with_teleporter"))); + + ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("linus", "person"))); + ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("hallway", "room"))); + ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("office", "room"))); + + std::vector predicates1; + predicates1.push_back(parser::pddl::fromStringPredicate("(person_at linus hallway)")); + predicates1.push_back(parser::pddl::fromStringPredicate("(person_at linus office)")); + + ASSERT_TRUE(problem_expert.addPredicates(predicates1)); + + ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("stallman", "person"))); + + std::vector predicates2; + predicates2.push_back(parser::pddl::fromStringPredicate("(person_at stallman hallway)")); + predicates2.push_back(parser::pddl::fromStringPredicate("(person_at stallman office)")); + + ASSERT_TRUE(problem_expert.updatePredicates(predicates2, predicates1)); + + ASSERT_FALSE(problem_expert.existPredicate(predicates1[0])); + ASSERT_FALSE(problem_expert.existPredicate(predicates1[1])); + ASSERT_TRUE(problem_expert.existPredicate(predicates2[0])); + ASSERT_TRUE(problem_expert.existPredicate(predicates2[1])); } TEST(problem_expert, addget_functions) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_charging.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -372,7 +364,7 @@ TEST(problem_expert, addget_functions) ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("wp1", "waypoint"))); ASSERT_TRUE(problem_expert.addInstance(parser::pddl::fromStringParam("wp2", "waypoint"))); - std::vector functions = problem_expert.getFunctions(); + std::unordered_set functions = problem_expert.getFunctions(); ASSERT_TRUE(functions.empty()); ASSERT_TRUE(problem_expert.addFunction(function_1)); @@ -407,9 +399,8 @@ TEST(problem_expert, addget_goals) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -426,8 +417,7 @@ TEST(problem_expert, addget_goals) plansys2_msgs::msg::Tree goal2; parser::pddl::fromString(goal2, "(and (robot_at r2d2 bedroom)(not(person_at paco kitchen)))"); ASSERT_EQ( - parser::pddl::toString( - goal2), "(and (robot_at r2d2 bedroom)(not (person_at paco kitchen)))"); + parser::pddl::toString(goal2), "(and (robot_at r2d2 bedroom)(not (person_at paco kitchen)))"); ASSERT_TRUE(problem_expert.setGoal(goal)); ASSERT_TRUE(problem_expert.setGoal(goal2)); @@ -438,8 +428,7 @@ TEST(problem_expert, addget_goals) const plansys2_msgs::msg::Tree & goal3 = problem_expert.getGoal(); ASSERT_EQ( - parser::pddl::toString( - goal3), "(and (robot_at r2d2 bedroom)(not (person_at paco kitchen)))"); + parser::pddl::toString(goal3), "(and (robot_at r2d2 bedroom)(not (person_at paco kitchen)))"); ASSERT_TRUE(problem_expert.clearGoal()); ASSERT_TRUE(problem_expert.clearGoal()); @@ -457,9 +446,8 @@ TEST(problem_expert, empty_goals) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -472,9 +460,8 @@ TEST(problem_expert, get_problem) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -520,14 +507,13 @@ TEST(problem_expert, get_problem) ASSERT_EQ( problem_expert.getProblem(), std::string("( define ( problem problem_1 )\n( :domain simple )\n") + - std::string("( :objects\n\tpaco - person\n\tr2d2 - robot\n") + - std::string("\tbedroom kitchen - room\n)\n") + - std::string("( :init\n\t( robot_at r2d2 bedroom )\n") + - std::string("\t( robot_at r2d2 kitchen )\n") + - std::string("\t( person_at paco bedroom )\n") + - std::string("\t( person_at paco kitchen )\n)\n") + - std::string("( :goal\n\t( and\n\t\t( robot_at r2d2 bedroom )\n\t\t") + - std::string("( person_at paco kitchen )\n\t))\n)\n")); + std::string("( :objects\n\tpaco - person\n\tr2d2 - robot\n") + + std::string("\tkitchen bedroom - room\n)\n") + + std::string("( :init\n\t( person_at paco kitchen )\n") + + std::string("\t( person_at paco bedroom )\n") + std::string("\t( robot_at r2d2 kitchen )\n") + + std::string("\t( robot_at r2d2 bedroom )\n)\n") + + std::string("( :goal\n\t( and\n\t\t( robot_at r2d2 bedroom )\n\t\t") + + std::string("( person_at paco kitchen )\n\t))\n)\n")); ASSERT_TRUE(problem_expert.clearKnowledge()); ASSERT_EQ(problem_expert.getPredicates().size(), 0); @@ -539,9 +525,8 @@ TEST(problem_expert, add_problem) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -550,29 +535,26 @@ TEST(problem_expert, add_problem) // Empty domain name std::ifstream problem_empty_domain_ifs(pkgpath + "/pddl/problem_empty_domain.pddl"); - std::string problem_empty_domain_str(( - std::istreambuf_iterator(problem_empty_domain_ifs)), - std::istreambuf_iterator()); + std::string problem_empty_domain_str( + (std::istreambuf_iterator(problem_empty_domain_ifs)), std::istreambuf_iterator()); ASSERT_FALSE(problem_expert.addProblem(problem_empty_domain_str)); // Domain doesn't exist std::ifstream problem_charging_ifs(pkgpath + "/pddl/problem_charging.pddl"); - std::string problem_charging_str(( - std::istreambuf_iterator(problem_charging_ifs)), - std::istreambuf_iterator()); + std::string problem_charging_str( + (std::istreambuf_iterator(problem_charging_ifs)), std::istreambuf_iterator()); ASSERT_FALSE(problem_expert.addProblem(problem_charging_str)); // missing syntax causes std::runtime_error std::ifstream problem_unexpected_syntax_ifs(pkgpath + "/pddl/problem_unexpected_syntax.pddl"); - std::string problem_unexpected_syntax_str(( - std::istreambuf_iterator(problem_unexpected_syntax_ifs)), + std::string problem_unexpected_syntax_str( + (std::istreambuf_iterator(problem_unexpected_syntax_ifs)), std::istreambuf_iterator()); ASSERT_FALSE(problem_expert.addProblem(problem_unexpected_syntax_str)); std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_1.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_expert.addProblem(problem_str)); ASSERT_TRUE(problem_expert.isValidType("robot")); @@ -601,28 +583,22 @@ TEST(problem_expert, add_problem) ASSERT_FALSE(problem_expert.existInstance("m2")); ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(robot_at leia kitchen)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(robot_at leia kitchen)"))); ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(person_at jack bedroom)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(person_at jack bedroom)"))); ASSERT_EQ(parser::pddl::toString(problem_expert.getGoal()), "(and (robot_talk leia m1 jack))"); ASSERT_EQ( problem_expert.getProblem(), std::string("( define ( problem problem_1 )\n") + - std::string("( :domain simple )\n( :objects\n") + - std::string("\tjack alice - person\n") + - std::string("\tm1 - message\n") + - std::string("\tleia - robot\n") + - std::string("\tkitchen bedroom - room\n)\n") + - std::string("( :init\n\t( robot_at leia kitchen )\n") + - std::string("\t( person_at jack bedroom )\n") + - std::string("\t( = ( room_distance kitchen bedroom ) 10.0000000000 )\n)\n") + - std::string("( :goal\n\t( and\n\t\t( robot_talk leia m1 jack )\n\t))\n)\n")); + std::string("( :domain simple )\n( :objects\n") + std::string("\talice jack - person\n") + + std::string("\tm1 - message\n") + std::string("\tleia - robot\n") + + std::string("\tbedroom kitchen - room\n)\n") + + std::string("( :init\n\t( person_at jack bedroom )\n") + + std::string("\t( robot_at leia kitchen )\n") + + std::string("\t( = ( room_distance kitchen bedroom ) 10.0000000000 )\n)\n") + + std::string("( :goal\n\t( and\n\t\t( robot_talk leia m1 jack )\n\t))\n)\n")); ASSERT_TRUE(problem_expert.clearKnowledge()); ASSERT_EQ(problem_expert.getPredicates().size(), 0); @@ -630,22 +606,19 @@ TEST(problem_expert, add_problem) ASSERT_EQ(problem_expert.getInstances().size(), 0); } - TEST(problem_expert, add_problem_with_constants) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple_constants.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); std::ifstream problem_1_ifs(pkgpath + "/pddl/problem_simple_constants_1.pddl"); - std::string problem_1_str(( - std::istreambuf_iterator(problem_1_ifs)), - std::istreambuf_iterator()); + std::string problem_1_str( + (std::istreambuf_iterator(problem_1_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_expert.addProblem(problem_1_str)); ASSERT_TRUE(problem_expert.isValidType("robot")); @@ -671,23 +644,19 @@ TEST(problem_expert, add_problem_with_constants) ASSERT_FALSE(problem_expert.existInstance("m2")); ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(robot_at leia kitchen)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(robot_at leia kitchen)"))); ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(person_at jack bedroom)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(person_at jack bedroom)"))); ASSERT_EQ(parser::pddl::toString(problem_expert.getGoal()), "(and (robot_talk leia m1 jack))"); ASSERT_EQ( problem_expert.getProblem(), std::string("( define ( problem problem_1 )\n( :domain plansys2 )\n") + - std::string("( :objects\n\tm1 - message\n\tkitchen bedroom - room\n)\n") + - std::string("( :init\n\t( robot_at leia kitchen )\n") + - std::string("\t( person_at jack bedroom )\n)\n") + - std::string("( :goal\n\t( and\n\t\t( robot_talk leia m1 jack )\n\t))\n)\n")); + std::string("( :objects\n\tm1 - message\n\tbedroom kitchen - room\n)\n") + + std::string("( :init\n\t( person_at jack bedroom )\n") + + std::string("\t( robot_at leia kitchen )\n)\n") + + std::string("( :goal\n\t( and\n\t\t( robot_talk leia m1 jack )\n\t))\n)\n")); ASSERT_TRUE(problem_expert.clearKnowledge()); ASSERT_EQ(problem_expert.getPredicates().size(), 0); @@ -696,27 +665,23 @@ TEST(problem_expert, add_problem_with_constants) ASSERT_EQ( problem_expert.getProblem(), std::string("( define ( problem problem_1 )\n( :domain plansys2 )\n") + - std::string("( :objects\n)\n( :init\n)\n( :goal\n\t( and\n\t))\n)\n")); - + std::string("( :objects\n)\n( :init\n)\n( :goal\n\t( and\n\t))\n)\n")); std::ifstream problem_2_ifs(pkgpath + "/pddl/problem_simple_constants_2.pddl"); - std::string problem_2_str(( - std::istreambuf_iterator(problem_2_ifs)), - std::istreambuf_iterator()); + std::string problem_2_str( + (std::istreambuf_iterator(problem_2_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_expert.addProblem(problem_2_str)); ASSERT_NE(problem_1_str, problem_2_str); ASSERT_NE(problem_expert.getProblem(), problem_2_str); - ASSERT_EQ(problem_expert.getProblem(), problem_1_str); } TEST(problem_expert, is_goal_satisfied) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); @@ -741,8 +706,7 @@ TEST(problem_expert, is_goal_satisfied) ASSERT_FALSE(problem_expert.isGoalSatisfied(goal)); ASSERT_TRUE( - problem_expert.addPredicate( - parser::pddl::fromStringPredicate("(robot_talk leia m1 Jack)"))); + problem_expert.addPredicate(parser::pddl::fromStringPredicate("(robot_talk leia m1 Jack)"))); ASSERT_TRUE(problem_expert.isGoalSatisfied(goal)); } @@ -750,133 +714,141 @@ TEST(problem_expert, is_goal_satisfied) TEST(problem_expert, exist_predicate) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); - std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple_derived.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::ifstream domain_ifs(pkgpath + "/pddl/domain_derived.pddl"); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); - std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_1.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::ifstream problem_ifs(pkgpath + "/pddl/problem_derived.pddl"); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_expert.addProblem(problem_str)); + problem_expert.updateInferredPredicates(); ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(robot_at leia kitchen)"))); - ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(inferred-robot_at leia kitchen)"))); - ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(person_at jack bedroom)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(robot_at leia kitchen)"))); + ASSERT_TRUE(problem_expert.existInferredPredicate( + parser::pddl::fromStringPredicate("(inferred-robot_at leia kitchen)"))); ASSERT_TRUE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(inferred-person_at jack bedroom)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(person_at jack bedroom)"))); + ASSERT_TRUE(problem_expert.existInferredPredicate( + parser::pddl::fromStringPredicate("(inferred-person_at jack bedroom)"))); + ASSERT_FALSE(problem_expert.existInferredPredicate( + parser::pddl::fromStringPredicate("(inferred-person_at jack kitchen)"))); + ASSERT_FALSE(problem_expert.existInferredPredicate( + parser::pddl::fromStringPredicate("(inferred-robot_at leia bedroom)"))); + + problem_expert.removePredicate(parser::pddl::fromStringPredicate("(robot_at leia kitchen)")); + problem_expert.removePredicate(parser::pddl::fromStringPredicate("(person_at jack bedroom)")); + problem_expert.updateInferredPredicates(); + + ASSERT_FALSE(problem_expert.existInferredPredicate( + parser::pddl::fromStringPredicate("(inferred-person_at jack bedroom)"))); + ASSERT_FALSE(problem_expert.existInferredPredicate( + parser::pddl::fromStringPredicate("(inferred-robot_at leia kitchen)"))); ASSERT_FALSE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(inferred-person_at jack kitchen)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(person_at jack bedroom)"))); ASSERT_FALSE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(inferred-robot_at leia bedroom)"))); - - problem_expert.removePredicate( - parser::pddl::fromStringPredicate("(robot_at leia kitchen)")); - problem_expert.removePredicate( - parser::pddl::fromStringPredicate("(person_at jack bedroom)")); - - ASSERT_FALSE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(inferred-person_at jack bedroom)"))); - ASSERT_FALSE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(inferred-robot_at leia kitchen)"))); - ASSERT_FALSE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(person_at jack bedroom)"))); - ASSERT_FALSE( - problem_expert.existPredicate( - parser::pddl::fromStringPredicate( - "(robot_at leia kitchen)"))); + problem_expert.existPredicate(parser::pddl::fromStringPredicate("(robot_at leia kitchen)"))); } TEST(problem_expert, get_predicate_with_derived) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); - std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple_derived.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::ifstream domain_ifs(pkgpath + "/pddl/domain_derived.pddl"); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); plansys2::ProblemExpert problem_expert(domain_expert); - std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_1.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::ifstream problem_ifs(pkgpath + "/pddl/problem_derived.pddl"); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_expert.addProblem(problem_str)); - auto predicates = problem_expert.getPredicates(); - ASSERT_EQ(predicates.size(), 4); - std::vector predicates_names; - std::for_each( - predicates.begin(), predicates.end(), - [&](auto p) {predicates_names.push_back(parser::pddl::toString(p));} - ); + auto predicates = problem_expert.getInferredPredicates(); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-robot_at leia kitchen)")) != + predicates.end()); + + ASSERT_FALSE( + predicates.find(parser::pddl::fromStringPredicate( + "(inferred-exists-another-drone-at-drone_area drone123)")) != predicates.end()); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-person_at jack bedroom)")) != + predicates.end()); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate( + "(inferred-party jose jose jose jose jose turtlebot turtlebot livingroom)")) != + predicates.end()); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate( + "(inferred-exists-party-in-room livingroom)")) != predicates.end()); + ASSERT_TRUE( - std::find( - predicates_names.begin(), predicates_names.end(), "(inferred-robot_at leia kitchen)") != - predicates_names.end()); + predicates.find(parser::pddl::fromStringPredicate("(inferred-aerial rob1)")) != + predicates.end()); + ASSERT_TRUE( - std::find( - predicates_names.begin(), predicates_names.end(), "(person_at jack bedroom)") != - predicates_names.end()); + predicates.find(parser::pddl::fromStringPredicate("(inferred-aerial rob2)")) != + predicates.end()); + ASSERT_TRUE( - std::find( - predicates_names.begin(), predicates_names.end(), "(inferred-person_at jack bedroom)") != - predicates_names.end()); + predicates.find(parser::pddl::fromStringPredicate("(inferred-not_aerial leia)")) != + predicates.end()); + ASSERT_FALSE( - std::find( - predicates_names.begin(), predicates_names.end(), "(inferred-person_at jack kitchen)") != - predicates_names.end()); + predicates.find(parser::pddl::fromStringPredicate("(inferred-person_at jack kitchen)")) != + predicates.end()); + ASSERT_FALSE( - std::find( - predicates_names.begin(), predicates_names.end(), "(inferred-robot_at leia bedroom)") != - predicates_names.end()); + predicates.find(parser::pddl::fromStringPredicate("(inferred-robot_at leia bedroom)")) != + predicates.end()); - problem_expert.removePredicate( - parser::pddl::fromStringPredicate("(robot_at leia kitchen)")); - problem_expert.removePredicate( - parser::pddl::fromStringPredicate("(person_at jack bedroom)")); + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-drone123 drone123)")) != + predicates.end()); - predicates = problem_expert.getPredicates(); - std::vector predicate_names2; - std::for_each( - predicates.begin(), predicates.end(), - [&](auto p) {predicate_names2.push_back(parser::pddl::toString(p));} - ); + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-not-drone123 rob1)")) != + predicates.end()); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-same-drone rob1 rob1)")) != + predicates.end()); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-not-same-drone rob1 drone123)")) != + predicates.end()); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-exists-equal-drone rob1)")) != + predicates.end()); + + ASSERT_TRUE( + predicates.find(parser::pddl::fromStringPredicate("(inferred-exists-another-drone rob1)")) != + predicates.end()); + + ASSERT_TRUE( + problem_expert.removePredicate(parser::pddl::fromStringPredicate("(robot_at leia kitchen)"))); + ASSERT_TRUE( + problem_expert.removePredicate(parser::pddl::fromStringPredicate("(person_at jack bedroom)"))); + + predicates = problem_expert.getInferredPredicates(); ASSERT_FALSE( - std::find( - predicate_names2.begin(), predicate_names2.end(), "(inferred-person_at jack bedroom)") != - predicate_names2.end()); + predicates.find(parser::pddl::fromStringPredicate("(inferred-person_at jack bedroom)")) != + predicates.end()); ASSERT_FALSE( - std::find( - predicate_names2.begin(), predicate_names2.end(), "(inferred-robot_at leia kitchen)") != - predicate_names2.end()); + predicates.find(parser::pddl::fromStringPredicate("(inferred-robot_at leia kitchen)")) != + predicates.end()); } int main(int argc, char ** argv) diff --git a/plansys2_problem_expert/test/unit/utils_test.cpp b/plansys2_problem_expert/test/unit/utils_test.cpp index ce2a4fb1c..34875e117 100644 --- a/plansys2_problem_expert/test/unit/utils_test.cpp +++ b/plansys2_problem_expert/test/unit/utils_test.cpp @@ -12,185 +12,268 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include +#include #include +#include #include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" - +#include "plansys2_domain_expert/DomainExpert.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "plansys2_msgs/msg/knowledge.hpp" #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" - +#include "plansys2_pddl_parser/Utils.hpp" #include "plansys2_problem_expert/ProblemExpert.hpp" -#include "plansys2_domain_expert/DomainExpert.hpp" -#include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" #include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" #include "plansys2_problem_expert/Utils.hpp" -#include "plansys2_pddl_parser/Utils.hpp" - -#include "plansys2_msgs/msg/knowledge.hpp" TEST(utils, evaluate_and) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; std::string expression = "(and (patrolled wp1) (patrolled wp2))"; plansys2_msgs::msg::Tree goal; parser::pddl::fromString(goal, expression); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(goal, state), + std::make_tuple(true, false, 0, std::vector>{})); - predicates.push_back(parser::pddl::fromStringPredicate("(patrolled wp1)")); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)")); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(goal, state), + std::make_tuple(true, false, 0, std::vector>{})); - predicates.clear(); - predicates.push_back(parser::pddl::fromStringPredicate("(patrolled wp2)")); + state.clearPredicates(); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp2)")); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(goal, state), + std::make_tuple(true, false, 0, std::vector>{})); - predicates.push_back(parser::pddl::fromStringPredicate("(patrolled wp1)")); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)")); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(goal, state), + std::make_tuple(true, true, 0, std::vector>{})); } TEST(utils, evaluate_or) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(or (patrolled wp1) (patrolled wp2))", false, - plansys2_msgs::msg::Node::AND); + test_tree, "(or (patrolled wp1) (patrolled wp2))", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 0, std::vector>{})); - predicates.push_back(parser::pddl::fromStringPredicate("(patrolled wp1)")); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)")); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); - predicates.clear(); - predicates.push_back(parser::pddl::fromStringPredicate("(patrolled wp2)")); + state.clearPredicates(); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp2)")); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); - predicates.push_back(parser::pddl::fromStringPredicate("(patrolled wp1)")); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)")); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); } TEST(utils, evaluate_not) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(not (patrolled wp1))", false, - plansys2_msgs::msg::Node::AND); + test_tree, "(not (patrolled wp1))", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); - predicates.push_back(parser::pddl::fromStringPredicate("(patrolled wp1)")); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)")); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 0, std::vector>{})); plansys2_msgs::msg::Tree test_tree2; - parser::pddl::fromString( - test_tree2, "(not (= wp1 wp2))"); + parser::pddl::fromString(test_tree2, "(not (= wp1 wp2))"); test_tree2.nodes[2].node_type = plansys2_msgs::msg::Node::CONSTANT; test_tree2.nodes[3].node_type = plansys2_msgs::msg::Node::CONSTANT; ASSERT_EQ( - plansys2::evaluate(test_tree2, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree2, state), + std::make_tuple(true, true, 0, std::vector>{})); plansys2_msgs::msg::Tree test_tree3; - parser::pddl::fromString( - test_tree3, "(not (= wp1 wp1))"); + parser::pddl::fromString(test_tree3, "(not (= wp1 wp1))"); test_tree3.nodes[2].node_type = plansys2_msgs::msg::Node::CONSTANT; test_tree3.nodes[3].node_type = plansys2_msgs::msg::Node::CONSTANT; ASSERT_EQ( - plansys2::evaluate(test_tree3, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree3, state), + std::make_tuple(true, false, 0, std::vector>{})); + + plansys2_msgs::msg::Tree test_tree4; + parser::pddl::fromString(test_tree4, "(not (and (patrolled wp1) (patrolled wp2)))"); + ASSERT_EQ( + plansys2::evaluate(test_tree4, state), + std::make_tuple(true, true, 0, std::vector>{})); + + state.addInstance(parser::pddl::fromStringParam("wp1")); + state.addInstance(parser::pddl::fromStringParam("wp2", "waypoint")); + state.addInstance(parser::pddl::fromStringParam("wp3")); + + plansys2_msgs::msg::Tree test_tree5; + parser::pddl::fromString(test_tree5, "(not (and (patrolled wp1) (patrolled ?x)))"); + std::vector> params_tree5; + params_tree5.push_back({{"?x", "wp2"}}); + params_tree5.push_back({{"?x", "wp3"}}); + ASSERT_EQ(plansys2::evaluate(test_tree5, state), std::make_tuple(true, true, 0, params_tree5)); + + plansys2_msgs::msg::Tree test_tree6; + parser::pddl::fromString(test_tree6, "(not (and (patrolled ?x) (patrolled_2 ?x)))"); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled_2 wp0)")); + ASSERT_EQ( + plansys2::evaluate(test_tree6, state), + std::make_tuple(true, true, 0, std::vector>{})); + state.addPredicate(parser::pddl::fromStringPredicate("(patrolled_2 wp1)")); + state.addInstance(parser::pddl::fromStringParam("wp0")); + std::vector> params_tree6; + params_tree6.push_back({{"?x", "wp0"}}); + params_tree6.push_back({{"?x", "wp2"}}); + params_tree6.push_back({{"?x", "wp3"}}); + ASSERT_EQ(plansys2::evaluate(test_tree6, state), std::make_tuple(true, true, 0, params_tree6)); + + plansys2::State state_or; + plansys2_msgs::msg::Tree test_tree_or; + parser::pddl::fromString(test_tree_or, "(not (or (patrolled ?x) (patrolled_2 ?x)))"); + ASSERT_EQ( + plansys2::evaluate(test_tree_or, state_or), + std::make_tuple(true, true, 0, std::vector>{})); + + plansys2_msgs::msg::Tree test_tree_or_2; + parser::pddl::fromString(test_tree_or_2, "(not (or (patrolled wp1) (patrolled_2 wp1)))"); + ASSERT_EQ( + plansys2::evaluate(test_tree_or_2, state_or), + std::make_tuple(true, true, 0, std::vector>{})); + + plansys2_msgs::msg::Tree test_tree_or_3; + parser::pddl::fromString(test_tree_or_3, "(not (or (patrolled wp1) (patrolled_2 wp1)))"); + ASSERT_EQ( + plansys2::evaluate(test_tree_or_3, state_or), + std::make_tuple(true, true, 0, std::vector>{})); + + state_or.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)")); + state_or.addInstance(parser::pddl::fromStringParam("wp1")); + state_or.addInstance(parser::pddl::fromStringParam("wp2", "waypoint")); + state_or.addInstance(parser::pddl::fromStringParam("wp3")); + ASSERT_EQ( + plansys2::evaluate(test_tree_or_3, state_or), + std::make_tuple(true, false, 0, std::vector>{})); + + std::vector> params_tree_or; + params_tree_or.push_back({{"?x", "wp2"}}); + params_tree_or.push_back({{"?x", "wp3"}}); + ASSERT_EQ( + plansys2::evaluate(test_tree_or, state_or), std::make_tuple(true, true, 0, params_tree_or)); + + state_or.addPredicate(parser::pddl::fromStringPredicate("(patrolled_2 wp1)")); + state_or.addInstance(parser::pddl::fromStringParam("wp0")); + std::vector> params_tree_or_2; + params_tree_or_2.push_back({{"?x", "wp0"}}); + params_tree_or_2.push_back({{"?x", "wp2"}}); + params_tree_or_2.push_back({{"?x", "wp3"}}); + ASSERT_EQ( + plansys2::evaluate(test_tree_or, state_or), std::make_tuple(true, true, 0, params_tree_or_2)); + + plansys2::State state_exists; + plansys2_msgs::msg::Tree test_tree_exists; + parser::pddl::fromString( + test_tree_exists, "(not (exists (?x) (and (patrolled ?x) (patrolled_2 ?x))))"); + ASSERT_EQ( + plansys2::evaluate(test_tree_exists, state_exists), + std::make_tuple(true, true, 0, std::vector>{})); + + plansys2_msgs::msg::Tree test_tree_exists_2; + parser::pddl::fromString( + test_tree_exists_2, "(not (exists (?x) (and (patrolled ?x) (patrolled_2 ?y))))"); + ASSERT_EQ( + plansys2::evaluate(test_tree_exists_2, state_exists), + std::make_tuple(true, true, 0, std::vector>{})); + + state_exists.addPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)")); + state_exists.addInstance(parser::pddl::fromStringParam("wp1")); + std::vector> params_tree_exists; + params_tree_exists.push_back({{"?y", "wp1"}}); + ASSERT_EQ( + plansys2::evaluate(test_tree_exists_2, state_exists), + std::make_tuple(true, true, 0, params_tree_exists)); + + state_exists.addPredicate(parser::pddl::fromStringPredicate("(patrolled_2 wp1)")); + state_exists.addInstance(parser::pddl::fromStringParam("wp2", "waypoint")); + state_exists.addInstance(parser::pddl::fromStringParam("wp3")); + + std::vector> params_tree_exists2; + params_tree_exists2.push_back({{"?y", "wp2"}}); + params_tree_exists2.push_back({{"?y", "wp3"}}); + ASSERT_EQ( + plansys2::evaluate(test_tree_exists_2, state_exists), + std::make_tuple(true, true, 0, params_tree_exists2)); } TEST(utils, evaluate_predicate_use_state) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString(test_tree, "(patrolled wp1)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 0, std::vector>{})); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true, 0, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state, 0, true), + std::make_tuple(true, true, 0, std::vector>{})); - ASSERT_TRUE(plansys2::apply(test_tree, predicates, functions)); - ASSERT_EQ(predicates.size(), 1); - ASSERT_EQ(parser::pddl::toString(*predicates.begin()), "(patrolled wp1)"); + ASSERT_TRUE(plansys2::apply(test_tree, state)); + ASSERT_EQ(state.getPredicatesSize(), 1); + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)"))); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, false, true, 0, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state, 0, true), + std::make_tuple(true, false, 0, std::vector>{})); - ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, true, true, 0, true), - std::make_tuple(true, false, 0)); - ASSERT_TRUE(predicates.empty()); + ASSERT_TRUE(plansys2::apply(test_tree, state, 0, true)); + ASSERT_EQ(state.getPredicatesSize(), 0); } TEST(utils, evaluate_predicate_client) { - std::vector predicates; - std::vector functions; auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); auto domain_node = std::make_shared(); auto problem_node = std::make_shared(); @@ -214,7 +297,9 @@ TEST(utils, evaluate_predicate_client) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("bedroom", "room"))); @@ -230,16 +315,13 @@ TEST(utils, evaluate_predicate_client) plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(is_teleporter_destination bedroom)", false, - plansys2_msgs::msg::Node::AND); + test_tree, "(is_teleporter_destination bedroom)", false, plansys2_msgs::msg::Node::AND); ASSERT_FALSE(plansys2::check(test_tree, problem_client)); ASSERT_TRUE(plansys2::apply(test_tree, problem_client)); ASSERT_TRUE(plansys2::check(test_tree, problem_client)); - ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions, true, false, 0, true), - std::make_tuple(true, false, 0.0)); + ASSERT_TRUE(plansys2::apply(test_tree, problem_client, 0, true)); ASSERT_FALSE(plansys2::check(test_tree, problem_client)); finish = true; @@ -248,30 +330,27 @@ TEST(utils, evaluate_predicate_client) TEST(utils, evaluate_function_use_state) { - std::vector predicates; - std::vector functions; + plansys2::State state; auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(distance wp1 wp2)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(distance wp1 wp2)", false, plansys2_msgs::msg::Node::EXPRESSION); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0.0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0.0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (distance wp1 wp2) 1.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (distance wp1 wp2) 1.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 1.0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 1.0, std::vector>{})); } TEST(utils, evaluate_expression_ge) { - std::vector predicates; - std::vector functions; + plansys2::State state; auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); auto problem_client = std::make_shared(); @@ -279,152 +358,143 @@ TEST(utils, evaluate_expression_ge) parser::pddl::fromString(test_tree, "(>= (vx) 3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (vx) 2.9999)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) 2.9999)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 0, std::vector>{})); - functions[0].value = 4.0; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) 2.9999)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) 4.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); - functions[0].value = 3.0; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) 4.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) 3.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); } TEST(utils, evaluate_expression_gt) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(> (distance wp1 wp2) 3.0)", false, - plansys2_msgs::msg::Node::AND); + test_tree, "(> (distance wp1 wp2) 3.0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (distance wp1 wp2) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (distance wp1 wp2) 3.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 0, std::vector>{})); - functions[0].value = 3.00001; + state.removeFunction(parser::pddl::fromStringFunction("(= (distance wp1 wp2) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (distance wp1 wp2) 3.00001)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); } TEST(utils, evaluate_expression_le) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(<= (vx) -3.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(<= (vx) -3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (vx) -2.9999)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) -2.9999)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 0, std::vector>{})); - functions[0].value = -4.0; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) -2.9999)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) -4.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); - functions[0].value = -3.0; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) -4.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) -3.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); } TEST(utils, evaluate_expression_lt) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(< (distance wp1 wp2) -3.0)", false, - plansys2_msgs::msg::Node::AND); + test_tree, "(< (distance wp1 wp2) -3.0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (distance wp1 wp2) -3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (distance wp1 wp2) -3.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 0, std::vector>{})); - functions[0].value = -3.00001; + state.removeFunction(parser::pddl::fromStringFunction("(= (distance wp1 wp2) -3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (distance wp1 wp2) -3.00001)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); } TEST(utils, evaluate_expression_multiply) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString(test_tree, "(* (vx) 3.0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (vx) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) 3.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 9.0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 9.0, std::vector>{})); - functions[0].value = -0.001; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) -0.001)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, -0.003)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, -0.003, std::vector>{})); } TEST(utils, evaluate_expression_divide) { - std::vector predicates; - std::vector functions; + plansys2::State state; auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); auto problem_client = std::make_shared(); @@ -432,90 +502,83 @@ TEST(utils, evaluate_expression_divide) parser::pddl::fromString(test_tree, "(/ (vx) 3.0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (vx) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) 3.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 1.0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 1.0, std::vector>{})); - functions[0].value = -9.0; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) -9.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, -3.0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, -3.0, std::vector>{})); - // Divide by zero test_tree.nodes.clear(); parser::pddl::fromString(test_tree, "(/ (vx) 0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); } TEST(utils, evaluate_expression_add) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString(test_tree, "(+ (vx) 3.0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (vx) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) 3.0)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 6.0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 6.0, std::vector>{})); - functions[0].value = -0.001; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) 3.0)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) -0.001)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 2.999)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 2.999, std::vector>{})); } TEST(utils, evaluate_expression_subtract) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString(test_tree, "(- (vx) 3.0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); - functions.push_back(parser::pddl::fromStringFunction("(= (vx) 2.5)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) 2.5)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, -0.5)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, -0.5, std::vector>{})); - functions[0].value = -0.001; + state.removeFunction(parser::pddl::fromStringFunction("(= (vx) 2.5)")); + state.addFunction(parser::pddl::fromStringFunction("(= (vx) -0.001)")); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, -3.001)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, -3.001, std::vector>{})); } TEST(utils, evaluate_expression_invalid) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; // Unknown expression type plansys2_msgs::msg::Tree test_tree; @@ -523,8 +586,8 @@ TEST(utils, evaluate_expression_invalid) test_tree.nodes[0].expression_type = plansys2_msgs::msg::Node::UNKNOWN; ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); } TEST(utils, evaluate_expression_invalid_client) @@ -552,7 +615,9 @@ TEST(utils, evaluate_expression_invalid_client) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("leia", "robot"))); @@ -571,12 +636,11 @@ TEST(utils, evaluate_expression_invalid_client) plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(> (room_distance bedroom kitchen) 0)", false, - plansys2_msgs::msg::Node::AND); + test_tree, "(> (room_distance bedroom kitchen) 0)", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( plansys2::evaluate(test_tree, problem_client), - std::make_tuple(false, false, 0)); + std::make_tuple(false, false, 0, std::vector>{})); { rclcpp::Rate rate(10); @@ -588,12 +652,11 @@ TEST(utils, evaluate_expression_invalid_client) test_tree.nodes.clear(); parser::pddl::fromString( - test_tree, "(> 0 (room_distance bedroom kitchen))", false, - plansys2_msgs::msg::Node::AND); + test_tree, "(> 0 (room_distance bedroom kitchen))", false, plansys2_msgs::msg::Node::AND); ASSERT_EQ( plansys2::evaluate(test_tree, problem_client), - std::make_tuple(false, false, 0)); + std::make_tuple(false, false, 0, std::vector>{})); finish = true; t.join(); @@ -609,77 +672,64 @@ TEST(utils, evaluate_function_mod) plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(assign (vx) 3.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(assign (vx) 3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); parser::pddl::getPredicates(predicates_msg, test_tree); parser::pddl::getFunctions(functions_msg, test_tree); - auto predicates = plansys2::convertVector( - predicates_msg); - auto functions = plansys2::convertVector( - functions_msg); + auto predicates = + plansys2::convertVectorToUnorderedSet( + predicates_msg); + auto functions = + plansys2::convertVectorToUnorderedSet( + functions_msg); - ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(true, false, 3.0)); - ASSERT_EQ(functions[0].value, 0.0); + std::unordered_set instances; + plansys2::State state(instances, functions, predicates); ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions, true), - std::make_tuple(true, false, 3.0)); - ASSERT_EQ(functions[0].value, 3.0); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, false, 3.0, std::vector>{})); + ASSERT_EQ(state.getFunction(parser::pddl::fromStringFunction("(vx)"))->value, 0.0); + + ASSERT_TRUE(plansys2::apply(test_tree, state)); + ASSERT_EQ(state.getFunction(parser::pddl::fromStringFunction("(vx)"))->value, 3.0); test_tree.nodes.clear(); parser::pddl::fromString( - test_tree, "(increase (vx) 3.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(increase (vx) 3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); - ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions, true), - std::make_tuple(true, false, 6.0)); - ASSERT_EQ(functions[0].value, 6.0); + ASSERT_TRUE(plansys2::apply(test_tree, state)); + ASSERT_EQ(state.getFunction(parser::pddl::fromStringFunction("(vx)"))->value, 6.0); test_tree.nodes.clear(); parser::pddl::fromString( - test_tree, "(decrease (vx) 3.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(decrease (vx) 3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); - ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions, true), - std::make_tuple(true, false, 3.0)); - ASSERT_EQ(functions[0].value, 3.0); + ASSERT_TRUE(plansys2::apply(test_tree, state)); + ASSERT_EQ(state.getFunction(parser::pddl::fromStringFunction("(vx)"))->value, 3.0); test_tree.nodes.clear(); parser::pddl::fromString( - test_tree, "(scale-up (vx) 3.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(scale-up (vx) 3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); - ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions, true), - std::make_tuple(true, false, 9.0)); - ASSERT_EQ(functions[0].value, 9.0); + ASSERT_TRUE(plansys2::apply(test_tree, state)); + ASSERT_EQ(state.getFunction(parser::pddl::fromStringFunction("(vx)"))->value, 9.0); test_tree.nodes.clear(); parser::pddl::fromString( - test_tree, "(scale-down (vx) 3.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(scale-down (vx) 3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); - ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions, true), - std::make_tuple(true, false, 3.0)); - ASSERT_EQ(functions[0].value, 3.0); + ASSERT_TRUE(plansys2::apply(test_tree, state)); + ASSERT_EQ(state.getFunction(parser::pddl::fromStringFunction("(vx)"))->value, 3.0); // divide by zero test_tree.nodes.clear(); parser::pddl::fromString( - test_tree, "(scale-down (vx) 0.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(scale-down (vx) 0.0)", false, plansys2_msgs::msg::Node::EXPRESSION); - ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions, true), - std::make_tuple(false, false, 0.0)); - ASSERT_EQ(functions[0].value, 3.0); + ASSERT_TRUE(plansys2::apply(test_tree, state)); + ASSERT_EQ(state.getFunction(parser::pddl::fromStringFunction("(vx)"))->value, 3.0); } TEST(utils, evaluate_function_mod_client) @@ -707,15 +757,15 @@ TEST(utils, evaluate_function_mod_client) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("bedroom", "room"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("kitchen", "room"))); ASSERT_TRUE( - problem_client->addFunction( - plansys2::Function( - "(= (room_distance bedroom kitchen) 1.0)"))); + problem_client->addFunction(plansys2::Function("(= (room_distance bedroom kitchen) 1.0)"))); { rclcpp::Rate rate(10); @@ -730,11 +780,9 @@ TEST(utils, evaluate_function_mod_client) test_tree, "(assign (room_distance bedroom kitchen) 0)", false, plansys2_msgs::msg::Node::EXPRESSION); - ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, true), - std::make_tuple(true, false, 0)); - std::optional func = problem_client->getFunction( - "(room_distance bedroom kitchen)"); + ASSERT_TRUE(plansys2::apply(test_tree, problem_client)); + std::optional func = + problem_client->getFunction("(room_distance bedroom kitchen)"); ASSERT_TRUE(func.has_value()); ASSERT_EQ(func.value().value, 0.0); @@ -743,12 +791,9 @@ TEST(utils, evaluate_function_mod_client) test_tree, "(increase (room_distance bedroom kitchen) 10.0)", false, plansys2_msgs::msg::Node::EXPRESSION); - ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, true), - std::make_tuple(true, false, 10.0)); + ASSERT_TRUE(plansys2::apply(test_tree, problem_client)); func = problem_client->getFunction("(room_distance bedroom kitchen)"); ASSERT_TRUE(func.has_value()); - ASSERT_EQ(func.value().value, 10.0); finish = true; t.join(); @@ -756,21 +801,17 @@ TEST(utils, evaluate_function_mod_client) TEST(utils, evaluate_function_mod_invalid) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; // Unknown function modifier type plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString( - test_tree, "(assign (vx) 3.0)", false, - plansys2_msgs::msg::Node::EXPRESSION); + test_tree, "(assign (vx) 3.0)", false, plansys2_msgs::msg::Node::EXPRESSION); test_tree.nodes[0].node_type = plansys2_msgs::msg::Node::UNKNOWN; ASSERT_EQ( - plansys2::evaluate(test_tree, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); } TEST(utils, evaluate_function_mod_invalid_client) @@ -798,7 +839,9 @@ TEST(utils, evaluate_function_mod_invalid_client) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); plansys2_msgs::msg::Tree test_tree; @@ -808,7 +851,7 @@ TEST(utils, evaluate_function_mod_invalid_client) ASSERT_EQ( plansys2::evaluate(test_tree, problem_client), - std::make_tuple(false, false, 0)); + std::make_tuple(false, false, 0, std::vector>{})); { rclcpp::Rate rate(10); @@ -825,7 +868,7 @@ TEST(utils, evaluate_function_mod_invalid_client) ASSERT_EQ( plansys2::evaluate(test_tree, problem_client), - std::make_tuple(false, false, 0)); + std::make_tuple(false, false, 0, std::vector>{})); finish = true; t.join(); @@ -833,71 +876,61 @@ TEST(utils, evaluate_function_mod_invalid_client) TEST(utils, evaluate_number) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; parser::pddl::fromString(test_tree, "3.0", false, plansys2_msgs::msg::Node::EXPRESSION); ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions), - std::make_tuple(true, true, 3.0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 3.0, std::vector>{})); } TEST(utils, evaluate_invalid) { - std::vector predicates; - std::vector functions; - auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); + plansys2::State state; plansys2_msgs::msg::Tree test_tree; ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions), - std::make_tuple(true, true, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(true, true, 0, std::vector>{})); parser::pddl::fromString(test_tree, "(patrolled wp1)", false, plansys2_msgs::msg::Node::AND); test_tree.nodes.front().node_type = plansys2_msgs::msg::Node::UNKNOWN; ASSERT_EQ( - plansys2::evaluate(test_tree, problem_client, predicates, functions), - std::make_tuple(false, false, 0)); + plansys2::evaluate(test_tree, state), + std::make_tuple(false, false, 0, std::vector>{})); } TEST(utils, evaluate_exists) { - std::vector predicates; - std::vector functions; + plansys2::State state; auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); - auto problem_client = std::make_shared(); std::string expression = "(exists (?1 ?2) (and (robot_at rob1 ?1)(connected ?1 ?2)))"; plansys2_msgs::msg::Tree goal; parser::pddl::fromString(goal, expression); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(goal, state), + std::make_tuple(true, false, 0, std::vector>{})); - predicates.push_back(parser::pddl::fromStringPredicate("(robot_at rob1 bedroom)")); + state.addPredicate(parser::pddl::fromStringPredicate("(robot_at rob1 bedroom)")); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, true), - std::make_tuple(true, false, 0)); + plansys2::evaluate(goal, state), + std::make_tuple(true, false, 0, std::vector>{})); - predicates.push_back(parser::pddl::fromStringPredicate("(connected bedroom kitchen)")); + state.addPredicate(parser::pddl::fromStringPredicate("(connected bedroom kitchen)")); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, true), - std::make_tuple(true, true, 0)); + plansys2::evaluate(goal, state), + std::make_tuple(true, true, 0, std::vector>{})); } TEST(utils, evaluate_exists_client) { - std::vector predicates; - std::vector functions; auto test_node = rclcpp::Node::make_shared("test_problem_expert_node"); auto domain_node = std::make_shared(); auto problem_node = std::make_shared(); @@ -906,22 +939,22 @@ TEST(utils, evaluate_exists_client) std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); problem_node->set_parameter({"model_file", pkgpath + "/pddl/domain_exists.pddl"}); + problem_node->set_parameter({"problem_file", pkgpath + "/pddl/problem_simple_exists.pddl"}); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); rclcpp::experimental::executors::EventsExecutor exe; + exe.add_node(domain_node->get_node_base_interface()); exe.add_node(problem_node->get_node_base_interface()); bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("bedroom", "room"))); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("kitchen", "room"))); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("rob1", "robot"))); - { rclcpp::Rate rate(10); auto start = test_node->now(); @@ -935,24 +968,24 @@ TEST(utils, evaluate_exists_client) parser::pddl::fromString(goal, expression); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, false), - std::make_tuple(true, false, 0)); + plansys2::evaluate(goal, problem_client), + std::make_tuple(true, false, 0, std::vector>{})); ASSERT_TRUE( - problem_client->addPredicate( - parser::pddl::fromStringPredicate("(robot_at rob1 bedroom)"))); + problem_client->addPredicate(parser::pddl::fromStringPredicate("(robot_at rob1 bedroom)"))); ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, false), - std::make_tuple(true, false, 0)); + plansys2::evaluate(goal, problem_client), + std::make_tuple(true, false, 0, std::vector>{})); ASSERT_TRUE( - problem_client->addPredicate( - parser::pddl::fromStringPredicate("(connected bedroom kitchen)"))); + problem_client->addPredicate(parser::pddl::fromStringPredicate("(connected bedroom kitchen)"))); + // std::vector> expected_result = { + // {{"?1", "bedroom"}, {"?2", "kitchen"}}}; ASSERT_EQ( - plansys2::evaluate(goal, problem_client, predicates, functions, false, false), - std::make_tuple(true, true, 0)); + plansys2::evaluate(goal, problem_client), + std::make_tuple(true, true, 0, std::vector>{})); finish = true; t.join(); @@ -1000,14 +1033,17 @@ TEST(utils, get_action_from_string) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} + while (!finish) { + exe.spin_some(); + } }); std::string invalid_action_str = "(invalid r2d2 kitchen bedroom)"; ASSERT_EQ( domain_client->getAction( plansys2::get_action_name(invalid_action_str), - plansys2::get_action_params(invalid_action_str)), nullptr); + plansys2::get_action_params(invalid_action_str)), + nullptr); std::string action_str = "(teleport r2d2 kitchen bedroom)"; @@ -1027,22 +1063,15 @@ TEST(utils, get_action_from_string) expected->preconditions = test_tree; test_tree.nodes.clear(); - parser::pddl::fromString( - test_tree, - "(and (not(robot_at r2d2 kitchen))(robot_at r2d2 bedroom))"); + parser::pddl::fromString(test_tree, "(and (not(robot_at r2d2 kitchen))(robot_at r2d2 bedroom))"); expected->effects = test_tree; - std::shared_ptr actual = - domain_client->getAction( - plansys2::get_action_name(action_str), - plansys2::get_action_params(action_str)); + std::shared_ptr actual = domain_client->getAction( + plansys2::get_action_name(action_str), plansys2::get_action_params(action_str)); ASSERT_EQ(parser::pddl::nameActionsToString(actual), parser::pddl::nameActionsToString(expected)); ASSERT_EQ( - parser::pddl::toString(actual->preconditions), - parser::pddl::toString(expected->preconditions)); - ASSERT_EQ( - parser::pddl::toString(actual->effects), - parser::pddl::toString(expected->effects)); + parser::pddl::toString(actual->preconditions), parser::pddl::toString(expected->preconditions)); + ASSERT_EQ(parser::pddl::toString(actual->effects), parser::pddl::toString(expected->effects)); std::string durative_action_str = "(move r2d2 kitchen bedroom)"; @@ -1068,8 +1097,8 @@ TEST(utils, get_action_from_string) std::shared_ptr durative_actual = domain_client->getDurativeAction( - plansys2::get_action_name(durative_action_str), - plansys2::get_action_params(durative_action_str)); + plansys2::get_action_name(durative_action_str), + plansys2::get_action_params(durative_action_str)); ASSERT_EQ( parser::pddl::nameActionsToString(durative_actual), @@ -1104,17 +1133,13 @@ TEST(utils, get_action_from_string) parser::pddl::fromString( overall_expected->over_all_requirements, "(and (robot_at leia kitchen) (person_at Jack kitchen))"); - parser::pddl::fromString( - overall_expected->at_end_requirements, - "(and (person_at Jack kitchen))"); - parser::pddl::fromString( - overall_expected->at_end_effects, - "(and (robot_near_person leia Jack))"); + parser::pddl::fromString(overall_expected->at_end_requirements, "(and (person_at Jack kitchen))"); + parser::pddl::fromString(overall_expected->at_end_effects, "(and (robot_near_person leia Jack))"); std::shared_ptr overall_actual = domain_client->getDurativeAction( - plansys2::get_action_name(overall_action_str), - plansys2::get_action_params(overall_action_str)); + plansys2::get_action_name(overall_action_str), + plansys2::get_action_params(overall_action_str)); ASSERT_EQ( parser::pddl::toString(overall_actual->at_start_requirements), @@ -1154,45 +1179,796 @@ TEST(utils, get_name) ASSERT_EQ(plansys2::get_action_name(action_str), "move"); } -TEST(utils, replace_children_param) { +TEST(utils, unify_predicate) +{ + std::unordered_set predicates; + auto predicate1 = parser::pddl::fromStringPredicate("(predicate a b)"); + auto predicate2 = parser::pddl::fromStringPredicate("(predicate a c)"); + auto predicate3 = parser::pddl::fromStringPredicate("(predicate b c)"); + auto predicate4 = parser::pddl::fromStringPredicate("(predicate d c)"); + auto predicate5 = parser::pddl::fromStringPredicate("(predicate e c)"); + predicates.insert(predicate1); + predicates.insert(predicate2); + predicates.insert(predicate3); + predicates.insert(predicate4); + predicates.insert(predicate5); + + auto res = plansys2::unifyPredicate(predicate1, predicates); + ASSERT_EQ(std::get<0>(res), true); + ASSERT_EQ(std::get<1>(res).size(), 0); + + res = plansys2::unifyPredicate(parser::pddl::fromStringPredicate("(predicate ?x c)"), predicates); + std::vector> expected_param_values = { + {{"?x", "a"}}, {{"?x", "b"}}, {{"?x", "d"}}, {{"?x", "e"}}}; + std::sort(std::get<1>(res).begin(), std::get<1>(res).end()); + std::sort(expected_param_values.begin(), expected_param_values.end()); + ASSERT_EQ(std::get<0>(res), true); + ASSERT_EQ(std::get<1>(res), expected_param_values); + + res = + plansys2::unifyPredicate(parser::pddl::fromStringPredicate("(predicate ?x ?y)"), predicates); + expected_param_values = { + {{"?x", "a"}, {"?y", "b"}}, {{"?x", "a"}, {"?y", "c"}}, {{"?x", "b"}, {"?y", "c"}}, + {{"?x", "d"}, {"?y", "c"}}, {{"?x", "e"}, {"?y", "c"}}, + }; + std::sort(std::get<1>(res).begin(), std::get<1>(res).end()); + std::sort(expected_param_values.begin(), expected_param_values.end()); + ASSERT_EQ(std::get<0>(res), true); + ASSERT_EQ(std::get<1>(res), expected_param_values); + + res = plansys2::unifyPredicate(parser::pddl::fromStringPredicate("(predicate ?x f)"), predicates); + ASSERT_EQ(std::get<0>(res), false); + ASSERT_EQ(std::get<1>(res).size(), 0); + + res = plansys2::unifyPredicate(parser::pddl::fromStringPredicate("(predicate z f)"), predicates); + ASSERT_EQ(std::get<0>(res), false); + ASSERT_EQ(std::get<1>(res).size(), 0); +} + +TEST(utils, intersection_parameters) +{ + std::vector> vector1 = { + {{"?x", "a"}, {"?y", "a"}}, + {{"?x", "b"}, {"?y", "b"}}, + {{"?x", "c"}, {"?y", "c"}}, + {{"?x", "d"}, {"?y", "d"}}, + }; + std::vector> vector2 = { + {{"?x", "a"}}, + {{"?x", "b"}}, + {{"?x", "d"}}, + }; + std::vector> vector3 = { + {{"?x", "d"}, {"?y", "c"}}, + {{"?x", "d"}, {"?y", "d"}}, + }; + std::vector> vector4; + std::vector> vector5 = { + {{"?z", "a"}}, + {{"?z", "b"}}, + {{"?z", "d"}}, + }; + + std::vector> vector6 = { + {{"?a", "a"}, {"?b", "a"}, {"?z", "a"}}, + {{"?b", "a"}, {"?x", "b"}, {"?y", "b"}, {"?z", "b"}}, + {{"?a", "a"}, {"?b", "a"}, {"?c", "a"}, {"?z", "d"}}, + }; + + std::vector> res = + plansys2::mergeParamsValuesVector(vector1, vector2); + std::sort(res.begin(), res.end()); + std::vector> expected_intersection = { + {{"?x", "a"}, {"?y", "a"}}, + {{"?x", "b"}, {"?y", "b"}}, + {{"?x", "d"}, {"?y", "d"}}, + }; + ASSERT_EQ(res, expected_intersection); + + res = plansys2::mergeParamsValuesVector(vector1, vector3); + std::sort(res.begin(), res.end()); + expected_intersection = { + {{"?x", "d"}, {"?y", "d"}}, + }; + ASSERT_EQ(res, expected_intersection); + + res = plansys2::mergeParamsValuesVector(vector1, vector4); + ASSERT_EQ(res.size(), 0); + + res = plansys2::mergeParamsValuesVector(vector4, vector1); + std::sort(res.begin(), res.end()); + std::sort(expected_intersection.begin(), expected_intersection.end()); + ASSERT_EQ(res.size(), 0); + + expected_intersection = { + {{"?x", "a"}, {"?y", "a"}, {"?z", "a"}}, {{"?x", "a"}, {"?y", "a"}, {"?z", "b"}}, + {{"?x", "a"}, {"?y", "a"}, {"?z", "d"}}, {{"?x", "b"}, {"?y", "b"}, {"?z", "a"}}, + {{"?x", "b"}, {"?y", "b"}, {"?z", "b"}}, {{"?x", "b"}, {"?y", "b"}, {"?z", "d"}}, + {{"?x", "c"}, {"?y", "c"}, {"?z", "a"}}, {{"?x", "c"}, {"?y", "c"}, {"?z", "b"}}, + {{"?x", "c"}, {"?y", "c"}, {"?z", "d"}}, {{"?x", "d"}, {"?y", "d"}, {"?z", "a"}}, + {{"?x", "d"}, {"?y", "d"}, {"?z", "b"}}, {{"?x", "d"}, {"?y", "d"}, {"?z", "d"}}, + }; + res = plansys2::mergeParamsValuesVector(vector1, vector5); + std::sort(res.begin(), res.end()); + std::sort(expected_intersection.begin(), expected_intersection.end()); + ASSERT_EQ(res, expected_intersection); + + expected_intersection = { + {{"?x", "a"}, {"?y", "a"}, {"?z", "a"}}, {{"?x", "b"}, {"?y", "b"}, {"?z", "a"}}, + {{"?x", "c"}, {"?y", "c"}, {"?z", "a"}}, {{"?x", "d"}, {"?y", "d"}, {"?z", "a"}}, + {{"?x", "a"}, {"?y", "a"}, {"?z", "b"}}, {{"?x", "b"}, {"?y", "b"}, {"?z", "b"}}, + {{"?x", "c"}, {"?y", "c"}, {"?z", "b"}}, {{"?x", "d"}, {"?y", "d"}, {"?z", "b"}}, + {{"?x", "a"}, {"?y", "a"}, {"?z", "d"}}, {{"?x", "b"}, {"?y", "b"}, {"?z", "d"}}, + {{"?x", "c"}, {"?y", "c"}, {"?z", "d"}}, {{"?x", "d"}, {"?y", "d"}, {"?z", "d"}}, + }; + res = plansys2::mergeParamsValuesVector(vector5, vector1); + std::sort(res.begin(), res.end()); + std::sort(expected_intersection.begin(), expected_intersection.end()); + ASSERT_EQ(res, expected_intersection); + + expected_intersection = { + {{"?a", "a"}, {"?b", "a"}, {"?x", "a"}, {"?y", "a"}, {"?z", "a"}}, + {{"?a", "a"}, {"?b", "a"}, {"?c", "a"}, {"?x", "a"}, {"?y", "a"}, {"?z", "d"}}, + {{"?a", "a"}, {"?b", "a"}, {"?x", "b"}, {"?y", "b"}, {"?z", "a"}}, + {{"?b", "a"}, {"?x", "b"}, {"?y", "b"}, {"?z", "b"}}, + {{"?a", "a"}, {"?b", "a"}, {"?c", "a"}, {"?x", "b"}, {"?y", "b"}, {"?z", "d"}}, + {{"?a", "a"}, {"?b", "a"}, {"?x", "c"}, {"?y", "c"}, {"?z", "a"}}, + {{"?a", "a"}, {"?b", "a"}, {"?c", "a"}, {"?x", "c"}, {"?y", "c"}, {"?z", "d"}}, + {{"?a", "a"}, {"?b", "a"}, {"?x", "d"}, {"?y", "d"}, {"?z", "a"}}, + {{"?a", "a"}, {"?b", "a"}, {"?c", "a"}, {"?x", "d"}, {"?y", "d"}, {"?z", "d"}}, + }; + res = plansys2::mergeParamsValuesVector(vector1, vector6); + std::sort(res.begin(), res.end()); + std::sort(expected_intersection.begin(), expected_intersection.end()); + ASSERT_EQ(res, expected_intersection); +} + +TEST(utils, complement_parameters) +{ + auto predicate = parser::pddl::fromStringPredicate("(robot_at ?0 ?1)"); + predicate.parameters[0].type = "robot"; + predicate.parameters[1].type = "room"; + + std::unordered_set instances; + instances.insert(parser::pddl::fromStringParam("r2d2", "robot")); + instances.insert(parser::pddl::fromStringParam("leia", "robot")); + + instances.insert(parser::pddl::fromStringParam("kitchen", "room")); + instances.insert(parser::pddl::fromStringParam("bedroom", "room")); + instances.insert(parser::pddl::fromStringParam("garage", "room")); + + std::vector> vector = { + {{"?0", "r2d2"}, {"?1", "kitchen"}}, + {{"?0", "leia"}, {"?1", "garage"}}, + {{"?0", "leia"}, {"?1", "kitchen"}}, + }; + + auto result = complementParamsValuesVector( + {predicate.parameters[0], predicate.parameters[1]}, vector, instances); + + ASSERT_EQ(result.size(), 3); + std::vector> expected = { + {{"?0", "leia"}, {"?1", "bedroom"}}, + {{"?0", "r2d2"}, {"?1", "garage"}}, + {{"?0", "r2d2"}, {"?1", "bedroom"}}, + }; + ASSERT_EQ(result, expected); + + predicate.parameters[0].name = "?1"; + predicate.parameters[1].name = "?3"; + vector = { + {{"?1", "r2d2"}, {"?3", "kitchen"}}, + {{"?1", "leia"}, {"?3", "garage"}}, + {{"?1", "leia"}, {"?3", "kitchen"}}, + }; + + result = complementParamsValuesVector( + {predicate.parameters[0], predicate.parameters[1]}, vector, instances); + + ASSERT_EQ(result.size(), 3); + expected = { + {{"?1", "leia"}, {"?3", "bedroom"}}, + {{"?1", "r2d2"}, {"?3", "garage"}}, + {{"?1", "r2d2"}, {"?3", "bedroom"}}, + }; + ASSERT_EQ(result, expected); + + vector = { + {{"?1", "r2d2"}}, + }; + + result = complementParamsValuesVector({predicate.parameters[0]}, vector, instances); + + ASSERT_EQ(result.size(), 1); + expected = { + {{"?1", "leia"}}, + }; + ASSERT_EQ(result, expected); +} + +TEST(utils, apply) +{ + plansys2::State state; + + plansys2_msgs::msg::Tree tree1; + parser::pddl::fromString(tree1, "(and (patrolled wp1) (patrolled wp2))"); + + ASSERT_TRUE(plansys2::apply(tree1, state)); + ASSERT_EQ(state.getPredicatesSize(), 2); + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)"))); + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp2)"))); + + plansys2_msgs::msg::Tree tree2; + parser::pddl::fromString(tree2, "(and (not (patrolled wp1)))"); + ASSERT_TRUE(plansys2::apply(tree2, state)); + ASSERT_EQ(state.getPredicatesSize(), 1); + ASSERT_FALSE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)"))); + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp2)"))); + + plansys2_msgs::msg::Tree tree3; + parser::pddl::fromString(tree3, "(and (patrolled wp1) (not (patrolled wp2)) (patrolled wp3))"); + ASSERT_TRUE(plansys2::apply(tree3, state)); + ASSERT_EQ(state.getPredicatesSize(), 2); + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp1)"))); + ASSERT_FALSE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp2)"))); + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(patrolled wp3)"))); +} + +TEST(utils, apply_with_derived) +{ + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); + std::ifstream domain_ifs(pkgpath + "/pddl/domain_derived.pddl"); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); + + auto domain_expert = std::make_shared(domain_str); + plansys2::ProblemExpert problem_expert(domain_expert); + + std::ifstream problem_ifs(pkgpath + "/pddl/problem_derived.pddl"); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); + ASSERT_TRUE(problem_expert.addProblem(problem_str)); + + auto state = problem_expert.getState(); + auto start_time = std::chrono::steady_clock::now(); + solveDerivedPredicates(state); + auto end_time = std::chrono::steady_clock::now(); + auto duration = + std::chrono::duration_cast>(end_time - start_time).count(); + ASSERT_GT(state.getInstances().size(), 0); + ASSERT_GT(state.getFunctions().size(), 0); + ASSERT_GT(state.getPredicates().size(), 0); + ASSERT_GT(state.getInferredPredicates().size(), 0); + ASSERT_GT(state.getDerivedPredicates().getEdgeNumber(), 0); + + ASSERT_TRUE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(robot_at leia kitchen)")) != state.getUnionPredicatesInferredPredicates().end()); + ASSERT_TRUE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(inferred-robot_at leia kitchen)")) != state.getUnionPredicatesInferredPredicates().end()); + ASSERT_FALSE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(robot_at leia bedroom)")) != state.getUnionPredicatesInferredPredicates().end()); + ASSERT_FALSE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(inferred-robot_at leia bedroom)")) != state.getUnionPredicatesInferredPredicates().end()); + + plansys2_msgs::msg::Tree tree1; + parser::pddl::fromString(tree1, "(and (not (robot_at leia kitchen)) (robot_at leia bedroom))"); + + auto apply_start_time = std::chrono::steady_clock::now(); + plansys2::apply(tree1, state); + auto apply_end_time = std::chrono::steady_clock::now(); + auto apply_duration = + std::chrono::duration_cast>(apply_end_time - apply_start_time) + .count(); + ASSERT_FALSE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(robot_at leia kitchen)")) != state.getUnionPredicatesInferredPredicates().end()); + ASSERT_FALSE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(inferred-robot_at leia kitchen)")) != state.getUnionPredicatesInferredPredicates().end()); + ASSERT_TRUE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(robot_at leia bedroom)")) != state.getUnionPredicatesInferredPredicates().end()); + ASSERT_TRUE( + state.getUnionPredicatesInferredPredicates().find(parser::pddl::fromStringPredicate( + "(inferred-robot_at leia bedroom)")) != state.getUnionPredicatesInferredPredicates().end()); +} + +TEST(utils, apply_with_derived_2) +{ std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); - std::string domain_file = pkgpath + "/pddl/domain_exists.pddl"; + std::ifstream domain_ifs(pkgpath + "/pddl/suave_domain.pddl"); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); + + auto domain_expert = std::make_shared(domain_str); + plansys2::ProblemExpert problem_expert(domain_expert); + + std::ifstream problem_ifs(pkgpath + "/pddl/suave_problem.pddl"); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); + ASSERT_TRUE(problem_expert.addProblem(problem_str)); + + auto state = problem_expert.getState(); + + auto start_time = std::chrono::steady_clock::now(); + solveDerivedPredicates(state); + auto end_time = std::chrono::steady_clock::now(); + auto duration = + std::chrono::duration_cast>(end_time - start_time).count(); + + ASSERT_GT(state.getInstances().size(), 0); + ASSERT_GT(state.getPredicates().size(), 0); + ASSERT_GT(state.getInferredPredicates().size(), 0); + ASSERT_GT(state.getDerivedPredicates().getEdgeNumber(), 0); + + plansys2_msgs::msg::Tree start_robot_effect; + parser::pddl::fromString(start_robot_effect, "(and (robot_started bluerov))"); + + plansys2::apply(start_robot_effect, state); + + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(robot_started bluerov)"))); + ASSERT_FALSE( + state.hasInferredPredicate(parser::pddl::fromStringPredicate("(robot_started bluerov)"))); + + auto action_reconfig_maintain = domain_expert->getAction( + "reconfigure", {"f_maintain_motion", "fd_unground", "fd_recover_thrusters"}); + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + + plansys2::apply(action_reconfig_maintain->effects, state); + ASSERT_FALSE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_maintain_motion fd_unground)"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_maintain_motion fd_unground)"))); + ASSERT_TRUE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_maintain_motion fd_recover_thrusters)"))); + + auto action_reconfig_generate = domain_expert->getAction( + "reconfigure", {"f_generate_search_path", "fd_unground", "fd_spiral_high"}); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + + plansys2::apply(action_reconfig_generate->effects, state); + ASSERT_FALSE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_generate_search_path fd_unground)"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_generate_search_path fd_unground)"))); + ASSERT_TRUE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_generate_search_path fd_spiral_high)"))); + + auto action_search_pipeline = domain_expert->getAction( + "search_pipeline", + {"a_search_pipeline", "pipeline", "bluerov", "fd_spiral_high", "fd_recover_thrusters"}); + ASSERT_TRUE(plansys2::check(action_search_pipeline->preconditions, state)); + + plansys2::apply(action_search_pipeline->effects, state); + ASSERT_TRUE(state.hasPredicate(parser::pddl::fromStringPredicate("(pipeline_found pipeline)"))); + + auto action_reconfig_follow = domain_expert->getAction( + "reconfigure", {"f_follow_pipeline", "fd_unground", "fd_follow_pipeline"}); + ASSERT_TRUE(plansys2::check(action_reconfig_follow->preconditions, state)); + + plansys2::apply(action_reconfig_follow->effects, state); + ASSERT_FALSE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_follow_pipeline fd_unground)"))); + ASSERT_TRUE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_follow_pipeline fd_follow_pipeline)"))); + + auto action_reconfig_generate_2 = domain_expert->getAction( + "reconfigure", {"f_generate_search_path", "fd_spiral_high", "fd_unground"}); + ASSERT_TRUE(plansys2::check(action_reconfig_generate_2->preconditions, state)); + + plansys2::apply(action_reconfig_generate_2->effects, state); + ASSERT_FALSE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_generate_search_path fd_spiral_high)"))); + ASSERT_TRUE(state.hasPredicate( + parser::pddl::fromStringPredicate("(system_in_mode f_generate_search_path fd_unground)"))); + + auto action_inspect_pipeline = domain_expert->getAction( + "inspect_pipeline", + {"a_inspect_pipeline", "pipeline", "bluerov", "fd_follow_pipeline", "fd_recover_thrusters"}); + ASSERT_TRUE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + plansys2::apply(action_inspect_pipeline->effects, state); + ASSERT_TRUE( + state.hasPredicate(parser::pddl::fromStringPredicate("(pipeline_inspected pipeline)"))); +} - std::ifstream domain_ifs(domain_file); +TEST(utils, apply_with_derived_suave_2) +{ + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); + std::ifstream domain_ifs(pkgpath + "/pddl/suave_domain2.pddl"); std::string domain_str( - (std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); - parser::pddl::Domain domain(domain_str); + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); + + auto domain_expert = std::make_shared(domain_str); + plansys2::ProblemExpert problem_expert(domain_expert); + + std::ifstream problem_ifs(pkgpath + "/pddl/suave_problem2.pddl"); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); + ASSERT_TRUE(problem_expert.addProblem(problem_str)); + + auto state = problem_expert.getState(); + + auto start_time = std::chrono::steady_clock::now(); + solveDerivedPredicates(state); + auto end_time = std::chrono::steady_clock::now(); + auto duration = + std::chrono::duration_cast>(end_time - start_time).count(); + + ASSERT_GT(state.getInstances().size(), 0); + ASSERT_GT(state.getPredicates().size(), 0); + ASSERT_GT(state.getInferredPredicates().size(), 0); + ASSERT_GT(state.getDerivedPredicates().getEdgeNumber(), 0); + + auto action_start_robot = domain_expert->getAction("start_robot", {"bluerov"}); + auto action_reconfig_maintain = + domain_expert->getAction("reconfigure1", {"f_maintain_motion", "fd_all_thrusters"}); + auto action_reconfig_generate = + domain_expert->getAction("reconfigure1", {"f_generate_search_path", "fd_spiral_high"}); + auto action_search_pipeline = + domain_expert->getAction("search_pipeline", {"pipeline", "bluerov"}); + auto action_reconfig_follow = + domain_expert->getAction("reconfigure1", {"f_follow_pipeline", "fd_follow_pipeline"}); + auto action_reconfig_generate2 = domain_expert->getAction( + "reconfigure2", {"f_generate_search_path", "fd_spiral_high", "fd_unground"}); + auto action_inspect_pipeline = + domain_expert->getAction("inspect_pipeline", {"pipeline", "bluerov"}); + + ASSERT_TRUE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(plansys2::apply(action_start_robot->effects, state)); + + ASSERT_FALSE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_all_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_recover_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_high f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_medium f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_low f_generate_search_path"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-fd_realisability fd_all_thrusters false_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate(parser::pddl::fromStringPredicate( + "inferred-fd_realisability fd_recover_thrusters false_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate(parser::pddl::fromStringPredicate( + "inferred-fdbetterutility fd_recover_thrusters fd_all_thrusters"))); + ASSERT_TRUE(state.hasInferredPredicate(parser::pddl::fromStringPredicate( + "inferred-fdbetterutility fd_all_thrusters fd_recover_thrusters"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_generate_search_path true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_maintain_motion true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_follow_pipeline true_boolean"))); + + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_maintain->effects, state)); + + ASSERT_FALSE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_all_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_recover_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_high f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_medium f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_low f_generate_search_path"))); + + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_generate->effects, state)); + + ASSERT_FALSE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_generate_search_path true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_maintain_motion true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_follow_pipeline true_boolean"))); + + ASSERT_TRUE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_search_pipeline->effects, state)); + + ASSERT_TRUE(plansys2::check(action_reconfig_follow->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_follow->effects, state)); + + ASSERT_TRUE(plansys2::check(action_reconfig_generate2->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_generate2->effects, state)); + + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_generate_search_path true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_maintain_motion true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_follow_pipeline true_boolean"))); + + ASSERT_TRUE(plansys2::check(action_inspect_pipeline->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_inspect_pipeline->effects, state)); +} - auto action = domain.actions.get("action_test"); +TEST(utils, apply_with_derived_suave_extended) +{ + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_problem_expert"); + std::ifstream domain_ifs(pkgpath + "/pddl/suave_domain_extended.pddl"); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); + + auto domain_expert = std::make_shared(domain_str); + plansys2::ProblemExpert problem_expert(domain_expert); + + std::ifstream problem_ifs(pkgpath + "/pddl/suave_problem_extended.pddl"); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); + ASSERT_TRUE(problem_expert.addProblem(problem_str)); + + auto state = problem_expert.getState(); + + auto start_time = std::chrono::steady_clock::now(); + solveDerivedPredicates(state); + auto end_time = std::chrono::steady_clock::now(); + auto duration = + std::chrono::duration_cast>(end_time - start_time).count(); + + ASSERT_GT(state.getInstances().size(), 0); + ASSERT_GT(state.getPredicates().size(), 0); + ASSERT_GT(state.getInferredPredicates().size(), 0); + ASSERT_GT(state.getDerivedPredicates().getEdgeNumber(), 0); + + auto action_start_robot = domain_expert->getAction("start_robot", {"bluerov"}); + auto action_reconfig_recharge_path_normal = + domain_expert->getAction("reconfigure1", {"generate_recharge_path", "normal"}); + auto action_reconfig_maintain = + domain_expert->getAction("reconfigure1", {"f_maintain_motion", "fd_all_thrusters"}); + auto action_recharge = domain_expert->getAction("recharge_battery", {"bluerov"}); + auto action_reconfig_generate = + domain_expert->getAction("reconfigure1", {"f_generate_search_path", "fd_spiral_high"}); + auto action_reconfig_recharge_path_unground = + domain_expert->getAction("reconfigure2", {"generate_recharge_path", "normal", "fd_unground"}); + auto action_search_pipeline = + domain_expert->getAction("search_pipeline", {"pipeline", "bluerov"}); + auto action_reconfig_follow = + domain_expert->getAction("reconfigure1", {"f_follow_pipeline", "fd_follow_pipeline"}); + auto action_reconfig_generate2 = domain_expert->getAction( + "reconfigure2", {"f_generate_search_path", "fd_spiral_high", "fd_unground"}); + auto action_inspect_pipeline = + domain_expert->getAction("inspect_pipeline", {"pipeline", "bluerov"}); + + ASSERT_TRUE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_recharge_path_normal->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_recharge->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_recharge_path_unground->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(plansys2::apply(action_start_robot->effects, state)); + + ASSERT_FALSE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_recharge_path_normal->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_recharge->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_recharge_path_unground->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_all_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_recover_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_high f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_medium f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_low f_generate_search_path"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-fd_realisability fd_all_thrusters false_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate(parser::pddl::fromStringPredicate( + "inferred-fd_realisability fd_recover_thrusters false_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate(parser::pddl::fromStringPredicate( + "inferred-fdbetterutility fd_recover_thrusters fd_all_thrusters"))); + ASSERT_TRUE(state.hasInferredPredicate(parser::pddl::fromStringPredicate( + "inferred-fdbetterutility fd_all_thrusters fd_recover_thrusters"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_generate_search_path true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_maintain_motion true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_follow_pipeline true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-battery_charged bluerov"))); + + ASSERT_TRUE(plansys2::check(action_reconfig_recharge_path_normal->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_recharge_path_normal->effects, state)); + + ASSERT_FALSE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_recharge_path_normal->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_recharge->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_recharge_path_unground->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_all_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_recover_thrusters f_maintain_motion"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_high f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_medium f_generate_search_path"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-solvesf fd_spiral_low f_generate_search_path"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-battery_charged bluerov"))); + + ASSERT_TRUE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_maintain->effects, state)); + + ASSERT_FALSE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_recharge_path_normal->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_recharge->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_recharge_path_unground->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_generate_search_path true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_maintain_motion true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_follow_pipeline true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active generate_recharge_path true_boolean"))); + + ASSERT_TRUE(plansys2::check(action_recharge->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_recharge->effects, state)); + + ASSERT_FALSE(plansys2::check(action_start_robot->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_recharge_path_normal->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_reconfig_maintain->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_recharge->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_TRUE(plansys2::check(action_reconfig_recharge_path_unground->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_FALSE(plansys2::check(action_inspect_pipeline->preconditions, state)); + + ASSERT_TRUE(state.hasPredicate( + parser::pddl::fromStringPredicate("qa_has_value obs_battery_level 1.0_decimal"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-battery_charged bluerov"))); + + ASSERT_TRUE(plansys2::check(action_reconfig_generate->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_generate->effects, state)); + + ASSERT_TRUE(plansys2::check(action_reconfig_recharge_path_unground->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_recharge_path_unground->effects, state)); + + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_generate_search_path true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_maintain_motion true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_follow_pipeline true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active generate_recharge_path true_boolean"))); + + ASSERT_TRUE(plansys2::check(action_search_pipeline->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_search_pipeline->effects, state)); + + ASSERT_TRUE(plansys2::check(action_reconfig_follow->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_follow->effects, state)); + + ASSERT_TRUE(plansys2::check(action_reconfig_generate2->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_reconfig_generate2->effects, state)); + + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_generate_search_path true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_maintain_motion true_boolean"))); + ASSERT_TRUE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active f_follow_pipeline true_boolean"))); + ASSERT_FALSE(state.hasInferredPredicate( + parser::pddl::fromStringPredicate("inferred-f_active generate_recharge_path true_boolean"))); + + ASSERT_TRUE(plansys2::check(action_inspect_pipeline->preconditions, state)); + ASSERT_TRUE(plansys2::apply(action_inspect_pipeline->effects, state)); +} + +TEST(utils, get_free_params) +{ plansys2_msgs::msg::Tree tree; - action->pre->getTree(tree, domain); - std::map replace; - replace["?1"] = "bedroom"; - replace["?2"] = "bathroom"; - plansys2_msgs::msg::Tree tree2 = plansys2::replace_children_param( - tree, - 1, - replace - ); - std::string str = parser::pddl::toString(tree2); - ASSERT_EQ( - str, - "(and (exists (bedroom) (and (robot_at ?0 bedroom)(charging_point_at bedroom)))" - "(and (> (battery_level ?0) 1.000000)(< (battery_level ?0) 200.000000)))"); - - auto action2 = domain.actions.get("action_test2"); + parser::pddl::fromString(tree, "(not (exists (?x) (and (patrolled ?x) (patrolled_2 ?y))))"); + auto free_params = plansys2::get_node_children_free_parameters(tree, tree.nodes[0]); + ASSERT_EQ(free_params.size(), 1); + ASSERT_EQ(free_params[0].name, "?y"); + + plansys2_msgs::msg::Tree tree2; + parser::pddl::fromString( + tree2, + "(and (predicateA ?a) (not (exists (?x) (and (patrolled ?x) (patrolled_2 ?y)))) (predicateA " + "?b))"); + free_params = plansys2::get_node_children_free_parameters(tree2, tree2.nodes[0]); + ASSERT_EQ(free_params.size(), 3); + ASSERT_EQ(free_params[0].name, "?a"); + ASSERT_EQ(free_params[1].name, "?y"); + ASSERT_EQ(free_params[2].name, "?b"); + + std::string expr = + R"((and + (robot_started ?r) + (exists (?a ?f1 ?f2 ?fd1 ?fd2) + (and + (inferred-Action ?a) + (= ?a a_search_pipeline) + (not (= ?f1 ?f2)) + (inferred-requiresF ?a ?f1) + (inferred-requiresF ?a ?f2) + (inferred-F_active ?f1 true_boolean) + (inferred-F_active ?f2 true_boolean) + (inferred-FunctionGrounding ?f1 ?fd1) + (inferred-FunctionGrounding ?f2 ?fd2) + (not + (exists (?fd1_b) + (and + (not (= ?fd1 ?fd1_b)) + (inferred-SolvesF ?fd1_b ?f1) + (not (inferred-Fd_realisability ?fd1_b false_boolean)) + (inferred-FdBetterUtility ?fd1_b ?fd1) + ) + ) + ) + (not + (exists (?fd2_b) + (and + (not (= ?fd2 ?fd2_b)) + (inferred-SolvesF ?fd2_b ?f2) + (not (inferred-Fd_realisability ?fd2_b false_boolean)) + (inferred-FdBetterUtility ?fd2_b ?fd2) + ) + ) + ) + ) + ) + (not (inferred-F_active f_follow_pipeline true_boolean)) +))"; plansys2_msgs::msg::Tree tree3; - action2->pre->getTree(tree3, domain); - plansys2_msgs::msg::Tree tree4 = plansys2::replace_children_param( - tree3, - 0, - replace - ); - std::string str2 = parser::pddl::toString(tree4); - ASSERT_EQ( - str2, - "(exists (bedroom bathroom) (and (robot_at ?0 bedroom)(connected bedroom bathroom)))"); + parser::pddl::fromString(tree3, expr); + auto free_params3 = plansys2::get_node_children_free_parameters(tree3, tree3.nodes[0]); + ASSERT_EQ(free_params3.size(), 1); + ASSERT_EQ(free_params3[0].name, "?r"); } int main(int argc, char ** argv)