From a0c28c2ed74a5c2b432c28328b283e15ae0e6f1e Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 22 Jul 2025 13:16:19 +0200 Subject: [PATCH 1/3] Added predicate_bt_paths and predicate_plugin_list to launch / blackboard / parameter Signed-off-by: Alberto Tudela --- .../plansys2_bringup_launch_monolithic.py | 38 ++++++++++------ plansys2_executor/CMakeLists.txt | 5 +++ .../plansys2_executor/ExecutorNode.hpp | 13 ++++++ .../src/plansys2_executor/ExecutorNode.cpp | 44 +++++++++++++++++++ 4 files changed, 86 insertions(+), 14 deletions(-) diff --git a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py index 0659eddb7..8661df823 100644 --- a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py +++ b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py @@ -34,6 +34,7 @@ def generate_launch_description(): start_action_bt_file = LaunchConfiguration('start_action_bt_file') end_action_bt_file = LaunchConfiguration('end_action_bt_file') bt_builder_plugin = LaunchConfiguration('bt_builder_plugin') + predicate_bt_paths = LaunchConfiguration('predicate_bt_paths') declare_model_file_cmd = DeclareLaunchArgument( 'model_file', @@ -52,22 +53,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( @@ -76,20 +77,28 @@ def generate_launch_description(): description='Behavior tree builder plugin.', ) + declare_predicate_bt_paths_cmd = DeclareLaunchArgument( + 'predicate_bt_paths', + default_value=os.path.join( + get_package_share_directory('plansys2_executor'), + 'behavior_trees', 'predicates_bt.yaml'), + description='Path to the YAML file with predicates behavior tree templates') + plansys2_node_cmd = Node( package='plansys2_bringup', executable='plansys2_node', 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, + 'predicate_bt_paths': predicate_bt_paths, + }, + params_file ]) # Create the launch description and populate @@ -102,6 +111,7 @@ def generate_launch_description(): ld.add_action(declare_bt_builder_plugin_cmd) ld.add_action(declare_namespace_cmd) ld.add_action(declare_params_file_cmd) + ld.add_action(declare_predicate_bt_paths_cmd) # Declare the launch options ld.add_action(plansys2_node_cmd) diff --git a/plansys2_executor/CMakeLists.txt b/plansys2_executor/CMakeLists.txt index 3ef589da3..c410c268e 100644 --- a/plansys2_executor/CMakeLists.txt +++ b/plansys2_executor/CMakeLists.txt @@ -26,6 +26,8 @@ find_package(pluginlib REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) +find_package(yaml-cpp REQUIRED) set(EXECUTOR_SOURCES src/plansys2_executor/ExecutorClient.cpp @@ -69,6 +71,7 @@ target_link_libraries(${PROJECT_NAME} ${std_srvs_TARGETS} PRIVATE ament_index_cpp::ament_index_cpp + yaml-cpp::yaml-cpp ) add_executable(executor_node @@ -176,5 +179,7 @@ ament_export_dependencies( behaviortree_cpp std_msgs std_srvs + yaml_cpp_vendor + yaml-cpp ) ament_package() diff --git a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp index a10c0c29f..863c07896 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" @@ -319,9 +320,21 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode */ void reset_groot_monitor(); + /** + * @brief Loads a mapping of predicates to behavior tree XML templates from a YAML file. + * + * @param[in] yaml_path Path to the YAML file containing predicate mappings. + * @param[out] std::unordered_map Mapping of predicate names to XML templates. + * @return Whether the loading was successful. + */ + bool load_predicate_bt_map( + const std::string & yaml_path, std::unordered_map & predicate_bt_map); + std::string action_bt_xml_; std::string start_action_bt_xml_; std::string end_action_bt_xml_; + std::string predicate_bt_paths_; + std::vector predicate_plugin_list_; pluginlib::ClassLoader bt_builder_loader_; std::shared_ptr domain_client_; diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index 3c6447857..a1f5e0a9b 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -34,6 +34,7 @@ #include "plansys2_msgs/msg/plan.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "yaml-cpp/yaml.h" #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" @@ -69,6 +70,9 @@ ExecutorNode::ExecutorNode(const rclcpp::NodeOptions & options) this->declare_parameter("default_action_bt_xml_filename", ""); this->declare_parameter("default_start_action_bt_xml_filename", ""); this->declare_parameter("default_end_action_bt_xml_filename", ""); + this->declare_parameter("predicate_bt_paths", ""); + this->declare_parameter>( + "predicate_plugins", std::vector({})); this->declare_parameter("bt_builder_plugin", ""); this->declare_parameter("action_time_precision", 3); this->declare_parameter("enable_dotgraph_legend", true); @@ -188,6 +192,18 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) end_action_bt_xml_.assign( std::istreambuf_iterator(end_action_bt_ifs), std::istreambuf_iterator()); + predicate_bt_paths_ = this->get_parameter("predicate_bt_paths").as_string(); + if (predicate_bt_paths_.empty()) { + RCLCPP_INFO( + get_logger(), + "No predicates behavior tree path specified, only symbolic predicates will be used"); + } + + 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); @@ -491,6 +507,15 @@ ExecutorNode::get_tree_from_plan(PlanRuntineInfo & runtime_info) 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 + std::unordered_map predicate_bt_map; + if (load_predicate_bt_map(predicate_bt_paths_, predicate_bt_map)) { + blackboard->set("predicate_bt_map", predicate_bt_map); + blackboard->set("predicate_plugin_list", predicate_plugin_list_); + } + // If a new tree is created, than the Groot2 Publisher must be destroyed reset_groot_monitor(); @@ -911,6 +936,24 @@ ExecutorNode::execution_cycle() } } +bool ExecutorNode::load_predicate_bt_map( + const std::string & yaml_path, std::unordered_map & predicate_bt_map) +{ + try { + YAML::Node config = YAML::LoadFile(yaml_path); + if (config["predicate_bt_paths"]) { + for (const auto & item : config["predicate_bt_paths"]) { + predicate_bt_map[item.first.as()] = item.second.as(); + } + } + } catch (const YAML::Exception & e) { + RCLCPP_ERROR(get_logger(), "Error reading file [%s]", yaml_path.c_str()); + return false; + } + + return true; +} + void ExecutorNode::add_groot_monitoring(BT::Tree * tree, uint16_t server_port) { // This logger publish status changes using Groot2 @@ -919,6 +962,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() From cd27027bf5f9043f40b11a756a6738380e79cd97 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Thu, 24 Jul 2025 14:34:32 +0200 Subject: [PATCH 2/3] Added PredicateBTExecutor Signed-off-by: Alberto Tudela --- plansys2_executor/CMakeLists.txt | 1 + .../plansys2_executor/PredicateBTExecutor.hpp | 82 +++++++++++++++++++ .../behavior_tree/wait_atstart_req_node.hpp | 14 ++++ .../plansys2_executor/PredicateBTExecutor.cpp | 80 ++++++++++++++++++ .../behavior_tree/wait_atstart_req_node.cpp | 66 +++++++++++++-- 5 files changed, 237 insertions(+), 6 deletions(-) create mode 100644 plansys2_executor/include/plansys2_executor/PredicateBTExecutor.hpp create mode 100644 plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp diff --git a/plansys2_executor/CMakeLists.txt b/plansys2_executor/CMakeLists.txt index c410c268e..a2a0379ba 100644 --- a/plansys2_executor/CMakeLists.txt +++ b/plansys2_executor/CMakeLists.txt @@ -35,6 +35,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/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/PredicateBTExecutor.cpp b/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp new file mode 100644 index 000000000..d629f0fb1 --- /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>("predicate_bt_map"); + predicate_plugin_list_ = blackboard_->get>("predicate_plugin_list"); + + // 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 From c0367de6f9b47f8c24981b412e2c069450095ad6 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Fri, 25 Jul 2025 12:03:44 +0200 Subject: [PATCH 3/3] Update predicates_bt_xml Signed-off-by: Alberto Tudela --- .../plansys2_bringup_launch_monolithic.py | 10 ---- plansys2_executor/CMakeLists.txt | 5 -- .../plansys2_executor/ExecutorNode.hpp | 16 ++---- .../src/plansys2_executor/ExecutorNode.cpp | 57 ++++++++----------- .../plansys2_executor/PredicateBTExecutor.cpp | 4 +- 5 files changed, 29 insertions(+), 63 deletions(-) diff --git a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py index 8661df823..99f5bdfd6 100644 --- a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py +++ b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py @@ -34,7 +34,6 @@ def generate_launch_description(): start_action_bt_file = LaunchConfiguration('start_action_bt_file') end_action_bt_file = LaunchConfiguration('end_action_bt_file') bt_builder_plugin = LaunchConfiguration('bt_builder_plugin') - predicate_bt_paths = LaunchConfiguration('predicate_bt_paths') declare_model_file_cmd = DeclareLaunchArgument( 'model_file', @@ -77,13 +76,6 @@ def generate_launch_description(): description='Behavior tree builder plugin.', ) - declare_predicate_bt_paths_cmd = DeclareLaunchArgument( - 'predicate_bt_paths', - default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'predicates_bt.yaml'), - description='Path to the YAML file with predicates behavior tree templates') - plansys2_node_cmd = Node( package='plansys2_bringup', executable='plansys2_node', @@ -96,7 +88,6 @@ def generate_launch_description(): '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, - 'predicate_bt_paths': predicate_bt_paths, }, params_file ]) @@ -111,7 +102,6 @@ def generate_launch_description(): ld.add_action(declare_bt_builder_plugin_cmd) ld.add_action(declare_namespace_cmd) ld.add_action(declare_params_file_cmd) - ld.add_action(declare_predicate_bt_paths_cmd) # Declare the launch options ld.add_action(plansys2_node_cmd) diff --git a/plansys2_executor/CMakeLists.txt b/plansys2_executor/CMakeLists.txt index a2a0379ba..8145d7aa8 100644 --- a/plansys2_executor/CMakeLists.txt +++ b/plansys2_executor/CMakeLists.txt @@ -26,8 +26,6 @@ find_package(pluginlib REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(yaml_cpp_vendor REQUIRED) -find_package(yaml-cpp REQUIRED) set(EXECUTOR_SOURCES src/plansys2_executor/ExecutorClient.cpp @@ -72,7 +70,6 @@ target_link_libraries(${PROJECT_NAME} ${std_srvs_TARGETS} PRIVATE ament_index_cpp::ament_index_cpp - yaml-cpp::yaml-cpp ) add_executable(executor_node @@ -180,7 +177,5 @@ ament_export_dependencies( behaviortree_cpp std_msgs std_srvs - yaml_cpp_vendor - yaml-cpp ) ament_package() diff --git a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp index 863c07896..4f0b96f85 100644 --- a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp +++ b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp @@ -320,21 +320,9 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode */ void reset_groot_monitor(); - /** - * @brief Loads a mapping of predicates to behavior tree XML templates from a YAML file. - * - * @param[in] yaml_path Path to the YAML file containing predicate mappings. - * @param[out] std::unordered_map Mapping of predicate names to XML templates. - * @return Whether the loading was successful. - */ - bool load_predicate_bt_map( - const std::string & yaml_path, std::unordered_map & predicate_bt_map); - std::string action_bt_xml_; std::string start_action_bt_xml_; std::string end_action_bt_xml_; - std::string predicate_bt_paths_; - std::vector predicate_plugin_list_; pluginlib::ClassLoader bt_builder_loader_; std::shared_ptr domain_client_; @@ -380,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/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index a1f5e0a9b..2a1952e21 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -34,7 +34,6 @@ #include "plansys2_msgs/msg/plan.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" -#include "yaml-cpp/yaml.h" #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" @@ -70,9 +69,6 @@ ExecutorNode::ExecutorNode(const rclcpp::NodeOptions & options) this->declare_parameter("default_action_bt_xml_filename", ""); this->declare_parameter("default_start_action_bt_xml_filename", ""); this->declare_parameter("default_end_action_bt_xml_filename", ""); - this->declare_parameter("predicate_bt_paths", ""); - this->declare_parameter>( - "predicate_plugins", std::vector({})); this->declare_parameter("bt_builder_plugin", ""); this->declare_parameter("action_time_precision", 3); this->declare_parameter("enable_dotgraph_legend", true); @@ -89,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(), @@ -192,13 +199,17 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) end_action_bt_xml_.assign( std::istreambuf_iterator(end_action_bt_ifs), std::istreambuf_iterator()); - predicate_bt_paths_ = this->get_parameter("predicate_bt_paths").as_string(); - if (predicate_bt_paths_.empty()) { - RCLCPP_INFO( - get_logger(), - "No predicates behavior tree path specified, only symbolic predicates will be used"); + // 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 << "]"); @@ -506,15 +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 - std::unordered_map predicate_bt_map; - if (load_predicate_bt_map(predicate_bt_paths_, predicate_bt_map)) { - blackboard->set("predicate_bt_map", predicate_bt_map); - blackboard->set("predicate_plugin_list", predicate_plugin_list_); - } + 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(); @@ -936,24 +943,6 @@ ExecutorNode::execution_cycle() } } -bool ExecutorNode::load_predicate_bt_map( - const std::string & yaml_path, std::unordered_map & predicate_bt_map) -{ - try { - YAML::Node config = YAML::LoadFile(yaml_path); - if (config["predicate_bt_paths"]) { - for (const auto & item : config["predicate_bt_paths"]) { - predicate_bt_map[item.first.as()] = item.second.as(); - } - } - } catch (const YAML::Exception & e) { - RCLCPP_ERROR(get_logger(), "Error reading file [%s]", yaml_path.c_str()); - return false; - } - - return true; -} - void ExecutorNode::add_groot_monitoring(BT::Tree * tree, uint16_t server_port) { // This logger publish status changes using Groot2 diff --git a/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp b/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp index d629f0fb1..78ba4cc4b 100644 --- a/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp +++ b/plansys2_executor/src/plansys2_executor/PredicateBTExecutor.cpp @@ -30,8 +30,8 @@ PredicateBTExecutor::PredicateBTExecutor(BT::Blackboard::Ptr blackboard) problem_client_ = blackboard_->get>("problem_client"); predicate_bt_map_ = - blackboard_->get>("predicate_bt_map"); - predicate_plugin_list_ = blackboard_->get>("predicate_plugin_list"); + blackboard_->get>("predicates_bt_xml"); + predicate_plugin_list_ = blackboard_->get>("predicate_plugins"); // Register the plugins for the behavior tree factory BT::SharedLibrary loader;