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 9a8839a..f48ebd7 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/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 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 26b6f11..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,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 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 diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp index 227a9b7..aa5b471 100644 --- a/btcpp_ros2_samples/src/sample_bt_executor.cpp +++ b/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(nodeBaseInterface()); - sub_ = node->create_subscription( + sub_ = node()->create_subscription( "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(); }