Merge pull request #65 from BehaviorTree/issues_61_63

Issues 61 and 63

behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp

@@ -56,14 +56,23 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
  const std::string& directory_path);
 
 /**
- * @brief Function to load BehaviorTree ROS plugins (or standard BT.CPP plugins) from a specific directory
+ * @brief Function to load a BehaviorTree ROS plugin (or standard BT.CPP plugins)
  *
+ * @param factory BehaviorTreeFactory to register the plugin into
+ * @param file_path Full path to the directory to search for BehaviorTree plugin
+ * @param params parameters passed to the ROS plugin
+ */
+void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path,
+ BT::RosNodeParams params);
+
+/** @brief Function to load all plugins from the specified package
+ *
+ * @param params ROS parameters that contain the package name to load plugins from
  * @param factory BehaviorTreeFactory to register the plugins into
- * @param directory_path Full path to the directory to search for BehaviorTree plugins
- * @param params parameters passed to the ROS plugins
+ * @param node node pointer that is shared with the ROS based Behavior plugins
  */
-void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
- BT::RosNodeParams params);
+void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+  rclcpp::Node::SharedPtr node);
 
 /**
  * @brief Function to register all Behaviors and BehaviorTrees from user specified packages

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

@@ -51,20 +51,25 @@ class TreeExecutionServer
  /**
  * @brief Gets the NodeBaseInterface of node_.
  * @details This function exists to allow running TreeExecutionServer as a component in a composable node container.
- *
- * @return A shared_ptr to the NodeBaseInterface of node_.
+ * The name of this method shall not change to work properly with the component composer.
  */
- rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface();
+ rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface();
+
+ /// @brief Gets the rclcpp::Node pointer
+ rclcpp::Node::SharedPtr node();
 
- // name of the tree being executed
+ /// @brief Name of the tree being executed
  const std::string& currentTreeName() const;
 
- // tree being executed, nullptr if it doesn't exist yet.
+ /// @brief Tree being executed, nullptr if it doesn't exist, yet.
  BT::Tree* currentTree();
 
- // pointer to the global blackboard
+ /// @brief Pointer to the global blackboard
  BT::Blackboard::Ptr globalBlackboard();
 
+ /// @brief Pointer to the global blackboard
+ BT::BehaviorTreeFactory& factory();
+
 protected:
  /**
  * @brief Callback invoked after the tree is created.
@@ -118,6 +123,18 @@ class TreeExecutionServer
  return std::nullopt;
  }
 
+protected:
+ /**
+ * @brief Method to register the trees and BT custom nodes.
+ * It will call registerNodesIntoFactory().
+ *
+ * This callback will invoked asynchronously when this rclcpp Node is attached
+ * to a rclcpp Executor.
+ * Alternatively, it can be called synchronously in the constructor of a
+ * __derived__ class of TreeExecutionServer.
+ */
+ void executeRegistration();
+
 private:
  struct Pimpl;
  std::unique_ptr<Pimpl> p_;

behaviortree_ros2/src/bt_utils.cpp

@@ -93,53 +93,42 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
  }
 }
 
-void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
-  BT::RosNodeParams params)
+void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path,
+ BT::RosNodeParams params)
 {
- using std::filesystem::directory_iterator;
-
- for(const auto& entry : directory_iterator(directory_path))
+ const auto filename = file_path.filename();
+ try
  {
- const auto filename = entry.path().filename();
- if(entry.path().extension() == ".so")
+ BT::SharedLibrary loader(file_path.string());
+ if(loader.hasSymbol(BT::PLUGIN_SYMBOL))
  {
- try
- {
- BT::SharedLibrary loader(entry.path().string());
- if(loader.hasSymbol(BT::PLUGIN_SYMBOL))
- {
- typedef void (*Func)(BehaviorTreeFactory&);
- auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
- func(factory);
- }
- else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL))
- {
- typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&);
- auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
- func(factory, params);
- }
- else
- {
- RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str());
- continue;
- }
- RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str());
- }
- catch(const std::exception& e)
- {
- RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(),
- e.what());
- }
+ typedef void (*Func)(BehaviorTreeFactory&);
+ auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
+ func(factory);
+ }
+ else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL))
+ {
+ typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&);
+ auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
+ func(factory, params);
+ }
+ else
+ {
+ RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str());
+ return;
  }
+ RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str());
+ }
+ catch(const std::exception& ex)
+ {
+ RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(),
+ ex.what());
  }
 }
 
-void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
-  rclcpp::Node::SharedPtr node)
+void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+ rclcpp::Node::SharedPtr node)
 {
- // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml
- factory.clearRegisteredBehaviorTrees();
-
  BT::RosNodeParams ros_params;
  ros_params.nh = node;
  ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout);
@@ -153,9 +142,21 @@ void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& f
  {
  continue;
  }
- LoadRosPlugins(factory, plugin_directory, ros_params);
+ using std::filesystem::directory_iterator;
+
+ for(const auto& entry : directory_iterator(plugin_directory))
+ {
+ if(entry.path().extension() == ".so")
+ {
+ LoadPlugin(factory, entry.path(), ros_params);
+ }
+ }
  }
+}
 
+void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+ rclcpp::Node::SharedPtr node)
+{
  for(const auto& tree_dir : params.behavior_trees)
  {
  const auto tree_directory = GetDirectoryPath(tree_dir);

behaviortree_ros2/src/tree_execution_server.cpp

@@ -47,6 +47,9 @@ struct TreeExecutionServer::Pimpl
  std::string current_tree_name;
  std::shared_ptr<BT::Tree> tree;
  BT::Blackboard::Ptr global_blackboard;
+ bool factory_initialized_ = false;
+
+ rclcpp::WallTimer<rclcpp::VoidCallbackType>::SharedPtr single_shot_timer;
 
  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid,
  std::shared_ptr<const ExecuteTree::Goal> goal);
@@ -70,6 +73,22 @@ TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options)
 TreeExecutionServer::~TreeExecutionServer()
 {}
 
+void TreeExecutionServer::executeRegistration()
+{
+ // Before executing check if we have new Behaviors or Subtrees to reload
+ p_->factory.clearRegisteredBehaviorTrees();
+
+ p_->params = p_->param_listener->get_params();
+ // user defined method
+ registerNodesIntoFactory(p_->factory);
+ // load plugins from multiple directories
+ RegisterPlugins(p_->params, p_->factory, p_->node);
+ // load trees (XML) from multiple directories
+ RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
+
+ p_->factory_initialized_ = true;
+}
+
 TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
  : p_(new Pimpl(options))
 {
@@ -89,17 +108,31 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
  handle_accepted(std::move(goal_handle));
  });
 
- // register the users Plugins and BehaviorTree.xml files into the factory
- RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
- registerNodesIntoFactory(p_->factory);
+ // we use a wall timer to run asynchronously executeRegistration();
+ rclcpp::VoidCallbackType callback = [this]() {
+ if(!p_->factory_initialized_)
+ {
+ executeRegistration();
+ }
+ // we must cancel the timer after the first execution
+ p_->single_shot_timer->cancel();
+ };
+
+ p_->single_shot_timer =
+ p_->node->create_wall_timer(std::chrono::milliseconds(1), callback);
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-TreeExecutionServer::nodeBaseInterface()
+TreeExecutionServer::get_node_base_interface()
 {
  return p_->node->get_node_base_interface();
 }
 
+rclcpp::Node::SharedPtr TreeExecutionServer::node()
+{
+ return p_->node;
+}
+
 rclcpp_action::GoalResponse
 TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */,
  std::shared_ptr<const ExecuteTree::Goal> goal)
@@ -146,9 +179,7 @@ void TreeExecutionServer::execute(
  // Before executing check if we have new Behaviors or Subtrees to reload
  if(p_->param_listener->is_old(p_->params))
  {
- p_->params = p_->param_listener->get_params();
- RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
- registerNodesIntoFactory(p_->factory);
+ executeRegistration();
  }
 
  // Loop until something happens with ROS or the node completes
@@ -262,4 +293,9 @@ BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard()
  return p_->global_blackboard;
 }
 
+BT::BehaviorTreeFactory& TreeExecutionServer::factory()
+{
+ return p_->factory;
+}
+
 } // namespace BT

btcpp_ros2_samples/src/sample_bt_executor.cpp

@@ -28,8 +28,7 @@ class MyActionServer : public BT::TreeExecutionServer
  MyActionServer(const rclcpp::NodeOptions& options) : TreeExecutionServer(options)
  {
  // here we assume that the battery voltage is published as a std_msgs::msg::Float32
- auto node = std::dynamic_pointer_cast<rclcpp::Node>(nodeBaseInterface());
- sub_ = node->create_subscription<std_msgs::msg::Float32>(
+ sub_ = node()->create_subscription<std_msgs::msg::Float32>(
  "battery_level", 10, [this](const std_msgs::msg::Float32::SharedPtr msg) {
  // Update the global blackboard
  globalBlackboard()->set("battery_level", msg->data);
@@ -68,9 +67,9 @@ int main(int argc, char* argv[])
  // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning.
  rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false,
  std::chrono::milliseconds(250));
- exec.add_node(action_server->nodeBaseInterface());
+ exec.add_node(action_server->node());
  exec.spin();
- exec.remove_node(action_server->nodeBaseInterface());
+ exec.remove_node(action_server->node());
 
  rclcpp::shutdown();
 }