Migrate plansys2_bt_actions to BehaviorTree.ROS2 wrappers

plansys2_bringup/params/plansys2_params.yaml

@@ -1,9 +1,5 @@
-planner_client:
-  ros__parameters:
-    plan_solver_timeout: 15.0  
 planner:
   ros__parameters:
-    plan_solver_timeout: 15.0  
     plan_solver_plugins: ["POPF"]
     POPF:
       plugin: "plansys2/POPFPlanSolver"

plansys2_bt_actions/CMakeLists.txt

@@ -1,7 +1,7 @@
-cmake_minimum_required(VERSION 3.5)
-
 project(plansys2_bt_actions)
 
+cmake_minimum_required(VERSION 3.5)
+
 list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
 find_package(ament_cmake REQUIRED)
@@ -63,7 +63,7 @@ if(BUILD_TESTING)
 
   set(dependencies ${dependencies} plansys2_msgs test_msgs geometry_msgs)
 
-  add_subdirectory(test)
+  # add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp

@@ -28,6 +28,9 @@
 #include "plansys2_executor/ActionExecutorClient.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "behaviortree_ros2/ros_node_params.hpp"
+#include "behaviortree_ros2/plugins.hpp"
+
 namespace plansys2
 {
 
@@ -61,12 +64,14 @@ class BTAction : public plansys2::ActionExecutorClient
 private:
   BT::Tree tree_;
   BT::Blackboard::Ptr blackboard_;
+  std::string node_namespace_;
   std::string action_;
   std::string bt_xml_file_;
-  std::vector<std::string> plugin_list_;
   bool finished_;
   std::unique_ptr<BT::FileLogger2> bt_file_logger_;
   std::unique_ptr<BT::MinitraceLogger> bt_minitrace_logger_;
+
+  std::string createFullyQualifiedName(const std::string & ns, const std::string & name);
 };
 
 }  // namespace plansys2

plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp

@@ -407,4 +407,4 @@ class BtActionNode : public BT::ActionNodeBase
 
 }  // namespace plansys2
 
-#endif  // PLANSYS2_BT_ACTIONS__BTACTIONNODE_HPP_
+#endif  // PLANSYS2_BT_ACTIONS__BTACTIONNODE_HPP_

plansys2_bt_actions/package.xml

@@ -18,7 +18,7 @@
   <depend>plansys2_executor</depend>
   <depend>behaviortree_cpp</depend>
   <depend>action_msgs</depend>
-
+  
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp

@@ -34,35 +34,104 @@ BTAction::BTAction(
 : ActionExecutorClient(action, rate)
 {
   declare_parameter<std::string>("bt_xml_file", "");
-  declare_parameter<std::vector<std::string>>(
-    "plugins", std::vector<std::string>({}));
+  declare_parameter<std::vector<std::string>>("plugin_names", std::vector<std::string>());
+  declare_parameter<std::vector<std::string>>("plugin_asts", std::vector<std::string>());
   declare_parameter<bool>("bt_file_logging", false);
   declare_parameter<bool>("bt_minitrace_logging", false);
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 BTAction::on_configure(const rclcpp_lifecycle::State & previous_state)
 {
-  get_parameter("action_name", action_);
-  get_parameter("bt_xml_file", bt_xml_file_);
+  node_namespace_ = this->get_namespace();
+
+  this->get_parameter("action_name", action_);
+  this->get_parameter("bt_xml_file", bt_xml_file_);
 
-  RCLCPP_INFO_STREAM(get_logger(), "action_name: [" << action_ << "]");
   RCLCPP_INFO_STREAM(get_logger(), "bt_xml_file: [" << bt_xml_file_ << "]");
 
-  auto plugin_lib_names = get_parameter("plugins").as_string_array();
-  for (auto plugin : plugin_lib_names) {
-    RCLCPP_INFO_STREAM(get_logger(), "plugin: [" << plugin << "]");
+  std::vector<std::string> plugin_names, plugin_asts;
+  this->get_parameter("plugin_names", plugin_names);
+  this->get_parameter("plugin_asts", plugin_asts);
+
+  if (plugin_names.size() != plugin_asts.size()) {
+    RCLCPP_ERROR(
+      this->get_logger(), "The number of plugin names does not match the number of plugin ASTs.");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
   }
 
   BT::SharedLibrary loader;
 
-  for (auto plugin : plugin_lib_names) {
-    factory_.registerFromPlugin(loader.getOSName(plugin));
+  for (size_t i = 0; i < plugin_names.size(); ++i) {
+    const std::string & plugin_name = plugin_names[i];
+    const std::string & plugin_ast = plugin_asts[i];
+
+    std::string plugin_path = loader.getOSName(plugin_name);
+    RCLCPP_DEBUG_STREAM(get_logger(), "Loading plugin: " << plugin_path);
+    RCLCPP_DEBUG_STREAM(get_logger(), "Plugin AST: " << plugin_ast);
+
+    // Try to load the plugin
+    try {
+      loader.load(plugin_path);
+    } catch (const std::exception & e) {
+      RCLCPP_WARN_STREAM(
+        get_logger(),
+        "Failed to load library: " << plugin_path << " - " << e.what());
+      continue;
+    }
+
+    // First try to register the node from the plugin
+    try {
+      if (loader.getSymbol("BT_RegisterNodesFromPlugin") != nullptr) {
+        RCLCPP_INFO_STREAM(get_logger(), "Registering node from plugin " << plugin_name);
+        loader.unload();
+        factory_.registerFromPlugin(plugin_path);
+        continue;
+      }
+    } catch (const std::exception & e) {
+      RCLCPP_DEBUG_STREAM(
+        get_logger(),
+        "Plugin " << plugin_name << " does not have symbol BT_RegisterNodesFromPlugin: " <<
+          e.what());
+    }
+
+    // Second try to register the ROS node from the plugin
+    try {
+      if (loader.getSymbol("BT_RegisterRosNodeFromPlugin") != nullptr) {
+        BT::RosNodeParams params;
+        auto nh = std::make_shared<rclcpp::Node>(plugin_name);
+
+        params.nh = nh;
+        params.default_port_value = createFullyQualifiedName(
+          node_namespace_,
+          plugin_ast);
+        loader.unload();
+        RCLCPP_INFO_STREAM(
+          get_logger(),
+          "Registering ROS node [" << params.default_port_value << "] from plugin [" << plugin_name <<
+            "]");
+        RegisterRosNode(factory_, plugin_path, params);
+        continue;
+      }
+    } catch (const std::exception & e) {
+      RCLCPP_DEBUG_STREAM(
+        get_logger(),
+        "Plugin " << plugin_name << " does not have symbol BT_RegisterRosNodeFromPlugin: " <<
+          e.what());
+    }
+
+    // If the plugin does not have any of the symbols, log an error
+    RCLCPP_ERROR_STREAM(
+      get_logger(),
+      "Plugin " << plugin_name <<
+        " does not have symbol BT_RegisterNodesFromPlugin nor BT_RegisterRosNodeFromPlugin");
+    loader.unload();
   }
 
   blackboard_ = BT::Blackboard::create();
   blackboard_->set("node", shared_from_this());
 
+
   return ActionExecutorClient::on_configure(previous_state);
 }
 
@@ -173,4 +242,28 @@ BTAction::do_work()
   }
 }
 
+// This function is used to create the fully qualified name of a node
+std::string BTAction::createFullyQualifiedName(const std::string & ns, const std::string & name)
+{
+  std::string fq_name;
+
+  if (!ns.empty()) {
+    if (ns.front() != '/') {
+      fq_name += "/";
+    }
+    fq_name += ns;
+
+    if (fq_name.back() == '/') {
+      fq_name.pop_back();
+    }
+  }
+
+  if (!fq_name.empty() && name.front() != '/') {
+    fq_name += "/";
+  }
+  fq_name += name;
+
+  return fq_name;
+}
+
 }  // namespace plansys2

plansys2_core/include/plansys2_core/PlanSolverBase.hpp

@@ -24,8 +24,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
-using namespace std::chrono_literals;
-
 namespace plansys2
 {
 
@@ -54,8 +52,7 @@ class PlanSolverBase
   */
   virtual std::optional<plansys2_msgs::msg::Plan> getPlan(
     const std::string & domain, const std::string & problem,
-    const std::string & node_namespace = "",
-    const rclcpp::Duration solver_timeout = 15s) = 0;
+    const std::string & node_namespace = "") = 0;
 
   /**
    * @brief Exposes a capability to validate a PDDL domain.

plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp

@@ -262,9 +262,10 @@ DomainExpert::getDurativeAction(const std::string & action, const std::vector<st
   unsigned i = 0;
 
   while (i < domain_->actions.size() && !found) {
+    bool is_durative_action =
+      dynamic_cast<parser::pddl::TemporalAction *>(domain_->actions[i]) != nullptr;
     parser::pddl::TemporalAction * action_obj =
       dynamic_cast<parser::pddl::TemporalAction *>(domain_->actions[i]);
-    bool is_durative_action = action_obj != nullptr;
 
     if (is_durative_action && action_obj->name == action_search) {
       found = true;

plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp

@@ -84,41 +84,21 @@ DomainReader::get_joint_domain() const
   }
   ret += ")\n\n";
 
-  bool functions = false;
-  bool constants = false;
-  bool types = false;
-
+  ret += "(:types\n";
   for (auto & domain : domains_) {
-    if (!domain.functions.empty()) {
-      functions = true;
-    }
     if (!domain.types.empty()) {
-      types = true;
-    }
-    if (!domain.constants.empty()) {
-      constants = true;
-    }
-  }
-
-  if (types) {
-    ret += "(:types\n";
-    for (auto & domain : domains_) {
-      if (!domain.types.empty()) {
-        ret += domain.types + "\n";
-      }
+      ret += domain.types + "\n";
     }
-    ret += ")\n\n";
   }
+  ret += ")\n\n";
 
-  if (constants) {
-    ret += "(:constants\n";
-    for (const auto & domain : domains_) {
-      if (!domain.constants.empty()) {
-        ret += domain.constants + "\n";
-      }
+  ret += "(:constants\n";
+  for (const auto & domain : domains_) {
+    if (!domain.constants.empty()) {
+      ret += domain.constants + "\n";
     }
-    ret += ")\n\n";
   }
+  ret += ")\n\n";
 
   ret += "(:predicates\n";
   std::set<std::string> preds_set;
@@ -131,15 +111,13 @@ DomainReader::get_joint_domain() const
   }
   ret += ")\n\n";
 
-  if (functions) {
-    ret += "(:functions\n";
-    for (auto & domain : domains_) {
-      if (!domain.functions.empty()) {
-        ret += domain.functions + "\n";
-      }
+  ret += "(:functions\n";
+  for (auto & domain : domains_) {
+    if (!domain.functions.empty()) {
+      ret += domain.functions + "\n";
     }
-    ret += ")\n\n";
   }
+  ret += ")\n\n";
 
   for (auto & domain : domains_) {
     for (auto & action : domain.actions) {

plansys2_domain_expert/test/pddl/domain_combined_processed.pddl

@@ -12,6 +12,9 @@ pickable_object
 room
 )
 
+(:constants
+)
+
 (:predicates
 (object_at_robot ?o - pickable_object ?r - robot)
 (object_at_room ?o - pickable_object ?ro - room)

plansys2_domain_expert/test/pddl/domain_simple_processed.pddl

@@ -9,6 +9,9 @@ room    - object
 teleporter_room - room
 )
 
+(:constants
+)
+
 (:predicates
 (person_at ?p - person ?ro - room)
 (robot_at ?r - robot ?ro - room)

plansys2_domain_expert/test/pddl/factory_processed.pddl

@@ -8,6 +8,9 @@
 	car - object
 )
 
+(:constants
+)
+
 (:predicates
 	( car_assembled ?car0 - car )
 	( is_assembly_zone ?zone0 - zone )
@@ -20,6 +23,9 @@
 	( robot_available ?robot0 - robot )
 )
 
+(:functions
+)
+
 (:durative-action move
   :parameters ( ?robot0 - robot ?zone1 - zone ?zone2 - zone )
   :duration ( = ?duration 5 )