diff --git a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py index 0659eddb7..99f5bdfd6 100644 --- a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py +++ b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py @@ -52,22 +52,22 @@ def generate_launch_description(): declare_action_bt_file_cmd = DeclareLaunchArgument( 'action_bt_file', default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_action_bt.xml'), + get_package_share_directory('plansys2_executor'), + 'behavior_trees', 'plansys2_action_bt.xml'), description='BT representing a PDDL action') declare_start_action_bt_file_cmd = DeclareLaunchArgument( 'start_action_bt_file', default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_start_action_bt.xml'), + get_package_share_directory('plansys2_executor'), + 'behavior_trees', 'plansys2_start_action_bt.xml'), description='BT representing a PDDL start action') declare_end_action_bt_file_cmd = DeclareLaunchArgument( 'end_action_bt_file', default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_end_action_bt.xml'), + get_package_share_directory('plansys2_executor'), + 'behavior_trees', 'plansys2_end_action_bt.xml'), description='BT representing a PDDL end action') declare_bt_builder_plugin_cmd = DeclareLaunchArgument( @@ -82,14 +82,14 @@ def generate_launch_description(): output='screen', namespace=namespace, parameters=[ - { - 'model_file': model_file, - 'default_action_bt_xml_filename': action_bt_file, - 'default_start_action_bt_xml_filename': start_action_bt_file, - 'default_end_action_bt_xml_filename': end_action_bt_file, - 'bt_builder_plugin': bt_builder_plugin, - }, - params_file + { + 'model_file': model_file, + 'default_action_bt_xml_filename': action_bt_file, + 'default_start_action_bt_xml_filename': start_action_bt_file, + 'default_end_action_bt_xml_filename': end_action_bt_file, + 'bt_builder_plugin': bt_builder_plugin, + }, + params_file ]) # Create the launch description and populate diff --git a/plansys2_executor/CMakeLists.txt b/plansys2_executor/CMakeLists.txt index 3ef589da3..8145d7aa8 100644 --- a/plansys2_executor/CMakeLists.txt +++ b/plansys2_executor/CMakeLists.txt @@ -33,6 +33,7 @@ set(EXECUTOR_SOURCES src/plansys2_executor/ActionExecutorClient.cpp src/plansys2_executor/ExecutorNode.cpp src/plansys2_executor/ComputeBT.cpp + src/plansys2_executor/PredicateBTExecutor.cpp src/plansys2_executor/behavior_tree/execute_action_node.cpp src/plansys2_executor/behavior_tree/wait_action_node.cpp src/plansys2_executor/behavior_tree/check_action_node.cpp diff --git a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp index a10c0c29f..4f0b96f85 100644 --- a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp +++ b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include "plansys2_domain_expert/DomainExpertClient.hpp" @@ -367,6 +368,10 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode PlanRuntineInfo runtime_info_; + // Predicates and their XML templates + std::vector predicate_plugin_list_; + std::unordered_map predicates_bt_xml_; + // Groot2 monitor std::unique_ptr groot_monitor_; }; diff --git a/plansys2_executor/include/plansys2_executor/PredicateBTExecutor.hpp b/plansys2_executor/include/plansys2_executor/PredicateBTExecutor.hpp new file mode 100644 index 000000000..858bbc327 --- /dev/null +++ b/plansys2_executor/include/plansys2_executor/PredicateBTExecutor.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2025 Alberto J. Tudela Roldán +// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga +// +// 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. + +#ifndef PLANSYS2_EXECUTOR__PREDICATEBTEXECUTOR_HPP_ +#define PLANSYS2_EXECUTOR__PREDICATEBTEXECUTOR_HPP_ + +#include +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/blackboard.h" +#include "plansys2_core/Types.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" + +namespace plansys2 +{ + +/** + * @class plansys2::PredicateBTExecutor + * @brief Class to execute behavior trees for checking predicates. + */ +class PredicateBTExecutor +{ +public: + /** + * @brief Constructor for PredicateBTExecutor. + * + * @param blackboard The blackboard to be used by the behavior trees. + */ + explicit PredicateBTExecutor(BT::Blackboard::Ptr blackboard); + + /** + * @brief Execute a predicate check using the specified behavior tree. + * + * @param predicate The predicate to check. + * @return bool Whether the predicate check was successful. + */ + bool check_predicate(const plansys2::Predicate & predicate); + +private: + /** + * @brief Execute the behavior tree for the given predicate. + * + * @param bt_xml_path The path to the behavior tree XML file. + * @return true If the behavior tree execution was successful, false otherwise. + */ + bool execute_bt(const std::string & bt_xml_path); + + /** + * @brief Set the predicate arguments in the blackboard. + * + * @param predicate The predicate whose arguments are to be set. + */ + void set_arguments_in_blackboard(const plansys2::Predicate & predicate); + + rclcpp::Logger logger_{rclcpp::get_logger("PredicateBTExecutor")}; + std::shared_ptr problem_client_; + + // Plugin list for predicates + std::vector predicate_plugin_list_; + // Map of predicate names to their corresponding behavior tree XML paths + std::unordered_map predicate_bt_map_; + // Factory for creating behavior trees and blackboard + BT::BehaviorTreeFactory factory_; + BT::Blackboard::Ptr blackboard_; +}; + +} // namespace plansys2 + +#endif // PLANSYS2_EXECUTOR__PREDICATEBTEXECUTOR_HPP_ diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp index b0b612fe5..479d635a1 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp @@ -24,6 +24,7 @@ #include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_problem_expert/Utils.hpp" +#include "plansys2_executor/PredicateBTExecutor.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" @@ -70,8 +71,21 @@ class WaitAtStartReq : public BT::ActionNodeBase } private: + /** + * @brief Checks if the at-start requirements are satisfied using a behavior tree. + * + * @param tree The PDDL tree to check against. + * @param problem_client The ProblemExpertClient to use for checking. + * @return true if the requirements are satisfied, false otherwise. + */ + bool check_with_bt( + const plansys2_msgs::msg::Tree & tree, + const std::shared_ptr & problem_client); + std::shared_ptr> action_map_; std::shared_ptr problem_client_; + std::unique_ptr predicate_bt_executor_; + rclcpp::Logger logger_{rclcpp::get_logger("WaitAtStartReq")}; }; } // namespace plansys2 diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index 3c6447857..2a1952e21 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -85,6 +85,17 @@ ExecutorNode::ExecutorNode(const rclcpp::NodeOptions & options) this->declare_parameter("enable_groot_monitoring", false); this->declare_parameter("server_port", 1800); + auto predicates = this->declare_parameter>( + "predicates", std::vector({})); + this->declare_parameter>( + "predicate_plugins", std::vector({})); + for (const auto & predicate : predicates) { + this->declare_parameter( + "predicates_bt_xml." + predicate, + ament_index_cpp::get_package_share_directory("plansys2_executor") + + "/behavior_trees/" + predicate + ".xml"); + } + execute_plan_action_server_ = rclcpp_action::create_server( this->get_node_base_interface(), this->get_node_clock_interface(), @@ -188,6 +199,22 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) end_action_bt_xml_.assign( std::istreambuf_iterator(end_action_bt_ifs), std::istreambuf_iterator()); + // Load the predicates and their XML templates + auto predicates = this->get_parameter("predicates").as_string_array(); + for (const auto & predicate : predicates) { + std::string bt_xml = this->get_parameter("predicates_bt_xml." + predicate).as_string(); + if (!bt_xml.empty()) { + predicates_bt_xml_[predicate] = bt_xml; + } else { + RCLCPP_WARN_STREAM( + get_logger(), "No XML template found for predicate [" << predicate << "]"); + } + } + predicate_plugin_list_ = get_parameter("predicate_plugins").as_string_array(); + for (auto plugin : predicate_plugin_list_) { + RCLCPP_INFO_STREAM(get_logger(), "plugin: [" << plugin << "]"); + } + dotgraph_pub_ = this->create_publisher("dot_graph", 1); execution_info_pub_ = create_publisher( "action_execution_info", 100); @@ -490,6 +517,11 @@ ExecutorNode::get_tree_from_plan(PlanRuntineInfo & runtime_info) blackboard->set("bt_loop_duration", std::chrono::milliseconds(200)); blackboard->set("server_timeout", std::chrono::milliseconds(250)); blackboard->set("wait_for_service_timeout", std::chrono::milliseconds(1000)); + // Set the map of predicates to behavior tree XML templates + // This map is used to execute predicates in the behavior tree + // And set the plugins for predicate execution + blackboard->set("predicates_bt_xml", predicates_bt_xml_); + blackboard->set("predicate_plugins", predicate_plugin_list_); // If a new tree is created, than the Groot2 Publisher must be destroyed reset_groot_monitor(); @@ -919,6 +951,7 @@ void ExecutorNode::add_groot_monitoring(BT::Tree * tree, uint16_t server_port) // Register common types JSON definitions BT::RegisterJsonDefinition(); BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition>(); } void ExecutorNode::reset_groot_monitor() diff --git a/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp b/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp new file mode 100644 index 000000000..78ba4cc4b --- /dev/null +++ b/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2025 Alberto J. Tudela Roldán +// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga +// +// 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 la + +#include "behaviortree_cpp/utils/shared_library.h" +#include "plansys2_executor/PredicateBTExecutor.hpp" +#include "plansys2_pddl_parser/Utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace plansys2 +{ + +PredicateBTExecutor::PredicateBTExecutor(BT::Blackboard::Ptr blackboard) +: blackboard_(blackboard) +{ + auto node = blackboard_->get("node"); + logger_ = node->get_logger(); + + problem_client_ = + blackboard_->get>("problem_client"); + predicate_bt_map_ = + blackboard_->get>("predicates_bt_xml"); + predicate_plugin_list_ = blackboard_->get>("predicate_plugins"); + + // Register the plugins for the behavior tree factory + BT::SharedLibrary loader; + for (auto plugin : predicate_plugin_list_) { + factory_.registerFromPlugin(loader.getOSName(plugin)); + } +} + +bool PredicateBTExecutor::check_predicate(const plansys2::Predicate & predicate) +{ + // Check if a specific behavior tree is defined for this predicate + // If so, check if the predicate exists symbolically and then execute the BT + // If not, just check if the predicate exists in the problem client + if (auto it = predicate_bt_map_.find(predicate.name); it != predicate_bt_map_.end()) { + RCLCPP_DEBUG( + logger_, + "Executing behavior tree for predicate: %s", parser::pddl::toString(predicate).c_str()); + set_arguments_in_blackboard(predicate); + return problem_client_->existPredicate(predicate) && execute_bt(it->second); + } else { + RCLCPP_DEBUG( + logger_, + "No specific behavior tree for predicate '%s', using default check.", + parser::pddl::toString(predicate).c_str()); + return problem_client_->existPredicate(predicate); + } +} + +void PredicateBTExecutor::set_arguments_in_blackboard(const plansys2::Predicate & predicate) +{ + for (int i = 0; i < predicate.parameters.size(); ++i) { + auto arg = predicate.parameters[i].name; + RCLCPP_DEBUG_STREAM(logger_, "Setting arg" << i << " [" << arg << "]"); + std::string argname = "arg" + std::to_string(i); + blackboard_->set(argname, arg); + } +} + +bool PredicateBTExecutor::execute_bt(const std::string & bt_xml_path) +{ + auto tree = factory_.createTreeFromFile(bt_xml_path, blackboard_); + BT::NodeStatus status = tree.rootNode()->executeTick(); + return status == BT::NodeStatus::SUCCESS; +} + +} // namespace plansys2 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..3aebe5c50 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 @@ -28,6 +28,9 @@ WaitAtStartReq::WaitAtStartReq( const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { + auto node = config().blackboard->get("node"); + logger_ = node->get_logger(); + action_map_ = config().blackboard->get>>( "action_map"); @@ -35,6 +38,8 @@ WaitAtStartReq::WaitAtStartReq( problem_client_ = config().blackboard->get>( "problem_client"); + + predicate_bt_executor_ = std::make_unique(config().blackboard); } BT::NodeStatus @@ -49,29 +54,27 @@ WaitAtStartReq::tick() return BT::NodeStatus::SUCCESS; } - auto node = config().blackboard->get("node"); - 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_); + bool check_as = check_with_bt(reqs_as, problem_client_); if (!check_as) { (*action_map_)[action].execution_error_info = "Error checking at start reqs"; RCLCPP_ERROR_STREAM( - node->get_logger(), + logger_, "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " << parser::pddl::toString(reqs_as)); return BT::NodeStatus::RUNNING; } - bool check_oa = check(reqs_oa, problem_client_); + bool check_oa = check_with_bt(reqs_oa, problem_client_); if (!check_oa) { (*action_map_)[action].execution_error_info = "Error checking over all reqs"; RCLCPP_ERROR_STREAM( - node->get_logger(), + logger_, "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " << parser::pddl::toString(reqs_oa)); @@ -81,4 +84,55 @@ WaitAtStartReq::tick() return BT::NodeStatus::SUCCESS; } +bool WaitAtStartReq::check_with_bt( + const plansys2_msgs::msg::Tree & tree, + const std::shared_ptr & problem_client) +{ + std::function eval_node; + eval_node = [&](const plansys2_msgs::msg::Node & node) -> bool + { + using Node = plansys2_msgs::msg::Node; + switch (node.node_type) { + case Node::AND: + { + for (const auto & child_id : node.children) { + const auto & child = tree.nodes[child_id]; + if (!eval_node(child)) { + return false; + } + } + return true; + } + case Node::OR: + { + for (const auto & child_id : node.children) { + const auto & child = tree.nodes[child_id]; + if (eval_node(child)) { + return true; + } + } + return false; + } + case Node::NOT: + { + const auto & child = tree.nodes[node.children[0]]; + return !eval_node(child); + } + case Node::PREDICATE: + { + return predicate_bt_executor_->check_predicate(node); + } + default: + return false; + } + }; + + if (tree.nodes.empty()) { + return true; + } + + const auto & root = tree.nodes[0]; + return eval_node(root); +} + } // namespace plansys2