From 3e150902b8430fdb916e4fdd925d9819431bb6d4 Mon Sep 17 00:00:00 2001 From: Ken Yoshizoe Date: Tue, 7 May 2024 03:34:04 +0900 Subject: [PATCH 1/3] fix registerNodesIntoFactory --- .../include/behaviortree_ros2/bt_utils.hpp | 14 +++++++++++--- .../behaviortree_ros2/tree_execution_server.hpp | 1 + behaviortree_ros2/src/bt_utils.cpp | 11 +++++------ behaviortree_ros2/src/tree_execution_server.cpp | 11 +++++------ 4 files changed, 22 insertions(+), 15 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp index d846996..54771be 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -64,6 +64,16 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, */ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, BT::RosNodeParams params); +/** + * @brief + * + * @param params ROS parameters that contain lists of packages to load + * plugins, ros_plugins and BehaviorTrees from + * @param factory BehaviorTreeFactory to register into + * @param node node pointer that is shared with the ROS based Behavior plugins + */ +void LoadPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node); /** * @brief Function to register all Behaviors and BehaviorTrees from user specified packages @@ -71,9 +81,7 @@ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directo * @param params ROS parameters that contain lists of packages to load * plugins, ros_plugins and BehaviorTrees from * @param factory BehaviorTreeFactory to register into - * @param node node pointer that is shared with the ROS based Behavior plugins */ -void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory, - rclcpp::Node::SharedPtr node); +void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory); } // namespace BT diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 9a8839a..97c690c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -121,6 +121,7 @@ class TreeExecutionServer private: struct Pimpl; std::unique_ptr p_; + bool bt_loaded_ = false; /** * @brief handle the goal requested: accept or reject. This implementation always accepts. diff --git a/behaviortree_ros2/src/bt_utils.cpp b/behaviortree_ros2/src/bt_utils.cpp index ce5e5cc..a525005 100644 --- a/behaviortree_ros2/src/bt_utils.cpp +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -134,17 +134,13 @@ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directo } } -void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory, - rclcpp::Node::SharedPtr node) +void LoadPlugins(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); ros_params.wait_for_server_timeout = ros_params.server_timeout; - for(const auto& plugin : params.plugins) { const auto plugin_directory = GetDirectoryPath(plugin); @@ -155,7 +151,10 @@ void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& f } LoadRosPlugins(factory, plugin_directory, ros_params); } +} +void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory) +{ 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..75facc2 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -88,10 +88,6 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) [this](const std::shared_ptr goal_handle) { 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); } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr @@ -144,11 +140,14 @@ void TreeExecutionServer::execute( auto action_result = std::make_shared(); // Before executing check if we have new Behaviors or Subtrees to reload - if(p_->param_listener->is_old(p_->params)) + if(p_->param_listener->is_old(p_->params) || !bt_loaded_) { + p_->factory.clearRegisteredBehaviorTrees(); p_->params = p_->param_listener->get_params(); - RegisterBehaviorTrees(p_->params, p_->factory, p_->node); + LoadPlugins(p_->params, p_->factory, p_->node); registerNodesIntoFactory(p_->factory); + RegisterBehaviorTrees(p_->params, p_->factory); + bt_loaded_ = true; } // Loop until something happens with ROS or the node completes From 2f21605fc3d191a4b34b6ee4e2477d9ad09d215c Mon Sep 17 00:00:00 2001 From: Ken Yoshizoe Date: Tue, 7 May 2024 18:46:53 +0900 Subject: [PATCH 2/3] update doxygen comments --- .../include/behaviortree_ros2/bt_utils.hpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp index 54771be..7985ac7 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -64,11 +64,11 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, */ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, BT::RosNodeParams params); + /** - * @brief + * @brief Function to load BehaviorTree plugins from a specific directory. * - * @param params ROS parameters that contain lists of packages to load - * plugins, ros_plugins and BehaviorTrees from + * @param params ROS parameters that contain lists of packages to load plugins, ros_plugins from * @param factory BehaviorTreeFactory to register into * @param node node pointer that is shared with the ROS based Behavior plugins */ @@ -76,10 +76,9 @@ void LoadPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory, rclcpp::Node::SharedPtr node); /** - * @brief Function to register all Behaviors and BehaviorTrees from user specified packages + * @brief Function to register all BehaviorTrees from user specified packages * - * @param params ROS parameters that contain lists of packages to load - * plugins, ros_plugins and BehaviorTrees from + * @param params ROS parameters that contain lists of packages to load BehaviorTrees from * @param factory BehaviorTreeFactory to register into */ void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory); From 4a18f2b872b3443e0829538a6803e98c31075a65 Mon Sep 17 00:00:00 2001 From: Ken Yoshizoe Date: Tue, 7 May 2024 19:06:20 +0900 Subject: [PATCH 3/3] add exceptions handling --- behaviortree_ros2/src/tree_execution_server.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 75facc2..79da694 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -145,7 +145,11 @@ void TreeExecutionServer::execute( p_->factory.clearRegisteredBehaviorTrees(); p_->params = p_->param_listener->get_params(); LoadPlugins(p_->params, p_->factory, p_->node); - registerNodesIntoFactory(p_->factory); + try { + registerNodesIntoFactory(p_->factory); + } catch (const std::exception& e) { + RCLCPP_ERROR(kLogger, "Failed to registerNodesIntoFactory(): %s", e.what()); + } RegisterBehaviorTrees(p_->params, p_->factory); bt_loaded_ = true; }