Add Predicate-Based Behavior Tree Executor

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

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

plansys2_executor/include/plansys2_executor/ExecutorNode.hpp

@@ -20,6 +20,7 @@
 #include <string>
 #include <map>
 #include <tuple>
+#include <unordered_map>
 #include <list>
 
 #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<std::string> predicate_plugin_list_;
+  std::unordered_map<std::string, std::string> predicates_bt_xml_;
+
   // Groot2 monitor
   std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };

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 <string>
+#include <unordered_map>
+
+#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<plansys2::ProblemExpertClient> problem_client_;
+
+  // Plugin list for predicates
+  std::vector<std::string> predicate_plugin_list_;
+  // Map of predicate names to their corresponding behavior tree XML paths
+  std::unordered_map<std::string, std::string> predicate_bt_map_;
+  // Factory for creating behavior trees and blackboard
+  BT::BehaviorTreeFactory factory_;
+  BT::Blackboard::Ptr blackboard_;
+};
+
+} // namespace plansys2
+
+#endif  // PLANSYS2_EXECUTOR__PREDICATEBTEXECUTOR_HPP_

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<plansys2::ProblemExpertClient> & problem_client);
+
   std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map_;
   std::shared_ptr<plansys2::ProblemExpertClient> problem_client_;
+  std::unique_ptr<PredicateBTExecutor> predicate_bt_executor_;
+  rclcpp::Logger logger_{rclcpp::get_logger("WaitAtStartReq")};
 };
 
 }  // namespace plansys2

plansys2_executor/src/plansys2_executor/ExecutorNode.cpp

@@ -85,6 +85,17 @@ ExecutorNode::ExecutorNode(const rclcpp::NodeOptions & options)
   this->declare_parameter<bool>("enable_groot_monitoring", false);
   this->declare_parameter<int>("server_port", 1800);
 
+  auto predicates = this->declare_parameter<std::vector<std::string>>(
+    "predicates", std::vector<std::string>({}));
+  this->declare_parameter<std::vector<std::string>>(
+    "predicate_plugins", std::vector<std::string>({}));
+  for (const auto & predicate : predicates) {
+    this->declare_parameter<std::string>(
+      "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<ExecutePlan>(
     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<char>(end_action_bt_ifs), std::istreambuf_iterator<char>());
 
+  // 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<std_msgs::msg::String>("dot_graph", 1);
   execution_info_pub_ = create_publisher<plansys2_msgs::msg::ActionExecutionInfo>(
     "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<builtin_interfaces::msg::Time>();
   BT::RegisterJsonDefinition<std_msgs::msg::Header>();
+  BT::RegisterJsonDefinition<std::unordered_map<std::string, std::string>>();
 }
 
 void ExecutorNode::reset_groot_monitor()

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<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
+  logger_ = node->get_logger();
+
+  problem_client_ =
+    blackboard_->get<std::shared_ptr<plansys2::ProblemExpertClient>>("problem_client");
+  predicate_bt_map_ =
+    blackboard_->get<std::unordered_map<std::string, std::string>>("predicates_bt_xml");
+  predicate_plugin_list_ = blackboard_->get<std::vector<std::string>>("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