From f6bd97f8ca295f36df7b10b431c2594dd8311fdc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 12 Jun 2024 07:04:13 +0000 Subject: [PATCH] Enable reloading BT xml file with same name (#4209) * Let BtActionServer overwrite xml * Make a ROS parameter for it * Rename flag to always reload BT xml file --------- Signed-off-by: Johannes Huemer Signed-off-by: Christoph Froehlich Co-authored-by: Johannes Huemer --- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 +++ .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 ++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index d81e0bca49..2ed7cc9b59 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -235,6 +235,9 @@ class BtActionServer // The timeout value for waiting for a service to response std::chrono::milliseconds wait_for_service_timeout_; + // should the BT be reloaded even if the same xml filename is requested? + bool always_reload_bt_xml_ = false; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 20d6702c12..c50cae2b77 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -60,6 +60,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + if (!node->has_parameter("always_reload_bt_xml")) { + node->declare_parameter("always_reload_bt_xml", false); + } if (!node->has_parameter("wait_for_service_timeout")) { node->declare_parameter("wait_for_service_timeout", 1000); } @@ -118,6 +121,7 @@ bool BtActionServer::on_configure() int wait_for_service_timeout; node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); + node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_); // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -174,8 +178,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Empty filename is default for backward compatibility auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; - // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == filename) { + // Use previous BT if it is the existing one and always reload flag is not set to true + if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) { RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); return true; }