diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp index d846996..e566758 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp +++ b/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 diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 0f86498..f48ebd7 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -123,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 p_; diff --git a/behaviortree_ros2/src/bt_utils.cpp b/behaviortree_ros2/src/bt_utils.cpp index ce5e5cc..65dc8de 100644 --- a/behaviortree_ros2/src/bt_utils.cpp +++ b/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); diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 212a2d5..64b08ae 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -47,6 +47,9 @@ struct TreeExecutionServer::Pimpl std::string current_tree_name; std::shared_ptr tree; BT::Blackboard::Ptr global_blackboard; + bool factory_initialized_ = false; + + rclcpp::WallTimer::SharedPtr single_shot_timer; rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr 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,9 +108,18 @@ 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 @@ -151,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