From 306ea18c6cbb5026a6dfcc6ba3947b4efe6ec42c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 25 Mar 2024 08:02:56 +0000 Subject: [PATCH 1/3] Let BtActionServer overwrite xml Co-authored-by: Johannes Huemer Signed-off-by: Johannes Huemer Signed-off-by: Christoph Froehlich --- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 ++- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 6 +++--- 2 files changed, 5 insertions(+), 4 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 331ca237c8..2fe6c06491 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 @@ -90,10 +90,11 @@ class BtActionServer /** * @brief Replace current BT with another one * @param bt_xml_filename The file containing the new BT, uses default filename if empty + * @param overwrite If true, the new BT will be reloaded even if the filename is the same * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ - bool loadBehaviorTree(const std::string & bt_xml_filename = ""); + bool loadBehaviorTree(const std::string & bt_xml_filename = "", bool overwrite = false); /** * @brief Getter function for BT Blackboard 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 12dffd2886..3d00851c4e 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 @@ -218,13 +218,13 @@ bool BtActionServer::on_cleanup() } template -bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) +bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename, bool overwrite) { // 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 overwrite flag is not set to true + if (!overwrite && current_bt_xml_filename_ == filename) { RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); return true; } From d8e531916825bcecad9ea61aa116056a7a3bb75f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 25 Mar 2024 08:03:10 +0000 Subject: [PATCH 2/3] Make a ROS parameter for it Signed-off-by: Christoph Froehlich --- .../include/nav2_behavior_tree/bt_action_server.hpp | 6 ++++-- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 ++++++-- 2 files changed, 10 insertions(+), 4 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 2fe6c06491..4508500a28 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 @@ -90,11 +90,10 @@ class BtActionServer /** * @brief Replace current BT with another one * @param bt_xml_filename The file containing the new BT, uses default filename if empty - * @param overwrite If true, the new BT will be reloaded even if the filename is the same * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ - bool loadBehaviorTree(const std::string & bt_xml_filename = "", bool overwrite = false); + bool loadBehaviorTree(const std::string & bt_xml_filename = ""); /** * @brief Getter function for BT Blackboard @@ -252,6 +251,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 overwrite_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 3d00851c4e..e4bbcbef70 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 @@ -67,6 +67,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("wait_for_service_timeout")) { node->declare_parameter("wait_for_service_timeout", 1000); } + if (!node->has_parameter("overwrite_xml")) { + node->declare_parameter("overwrite_xml", false); + } std::vector error_code_names = { "follow_path_error_code", @@ -164,6 +167,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("overwrite_xml", overwrite_xml_); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -218,13 +222,13 @@ bool BtActionServer::on_cleanup() } template -bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename, bool overwrite) +bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) { // 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 and overwrite flag is not set to true - if (!overwrite && current_bt_xml_filename_ == filename) { + if (!overwrite_xml_ && current_bt_xml_filename_ == filename) { RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); return true; } From 43bca51578a1adc1eedb32ebeceaf318a046ccb6 Mon Sep 17 00:00:00 2001 From: Johannes Huemer Date: Mon, 25 Mar 2024 08:04:12 +0000 Subject: [PATCH 3/3] Rename flag to always reload BT xml file Signed-off-by: Johannes Huemer --- .../include/nav2_behavior_tree/bt_action_server.hpp | 2 +- .../nav2_behavior_tree/bt_action_server_impl.hpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 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 4508500a28..2b323a6798 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 @@ -252,7 +252,7 @@ class BtActionServer std::chrono::milliseconds wait_for_service_timeout_; // should the BT be reloaded even if the same xml filename is requested? - bool overwrite_xml_ = false; + bool always_reload_bt_xml_ = false; // User-provided callbacks OnGoalReceivedCallback on_goal_received_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 e4bbcbef70..deaeea1bdc 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 @@ -64,12 +64,12 @@ BtActionServer::BtActionServer( if (!node->has_parameter("action_server_result_timeout")) { node->declare_parameter("action_server_result_timeout", 900.0); } + 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); } - if (!node->has_parameter("overwrite_xml")) { - node->declare_parameter("overwrite_xml", false); - } std::vector error_code_names = { "follow_path_error_code", @@ -167,7 +167,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("overwrite_xml", overwrite_xml_); + node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -227,8 +227,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 and overwrite flag is not set to true - if (!overwrite_xml_ && 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; }