From 746884370313e5bd309b62d65678143c928d201d Mon Sep 17 00:00:00 2001 From: Jad EL HAJJ Date: Wed, 17 Sep 2025 20:55:10 +0300 Subject: [PATCH 01/16] Fix/prevent warning when loading bt (#5494) * Added BT ID finder and used createTree to resolve btcpp warning Signed-off-by: Jad El Hajj * Support using either bt file path or bt id Signed-off-by: Jad El Hajj * Not necessarily a file name Signed-off-by: Jad El Hajj * fix logic Signed-off-by: Jad El Hajj * fix function definition Signed-off-by: Jad El Hajj * fixed unit test Signed-off-by: Jad El Hajj * fixed BB variable Signed-off-by: Jad El Hajj * fixed error msg Signed-off-by: Jad El Hajj * fixed variable and its getter naming Signed-off-by: Jad El Hajj * fixed definition Signed-off-by: Jad El Hajj * fix xml check Signed-off-by: Jad El Hajj * fix bt unit test Signed-off-by: Jad El Hajj * test Signed-off-by: Jad El Hajj * added back test Signed-off-by: Jad El Hajj * check bt id using the root or bt id Signed-off-by: Jad El Hajj * using tinyxml2 instead of regex Signed-off-by: Jad El Hajj * fixed var name Signed-off-by: Jad El Hajj * fixed var name Signed-off-by: Jad El Hajj * check if arg is already a BT ID Signed-off-by: Jad El Hajj * check if arg is already a BT ID Signed-off-by: Jad El Hajj * Test was failing due to same BT ID MainTree among all registred trees Signed-off-by: Jad El Hajj * Fixed error msg to be compliant Signed-off-by: Jad El Hajj * python linting Signed-off-by: Jad El Hajj * Removed unused createTreeFromFile since its replaced with createTree Signed-off-by: Jad El Hajj * PR fixes Signed-off-by: Jad El Hajj * Added new line at the end of BT xml Signed-off-by: Jad El Hajj * Should cover most of the cases Signed-off-by: Jad El Hajj * format Signed-off-by: Jad El Hajj * Fixed BT format Signed-off-by: Jad El Hajj * Removed redundant check Signed-off-by: Jad El Hajj * Allow usage of file paths with a warning, while keeping BT ID usage as the recommended option Signed-off-by: Jad El Hajj * Additional test Signed-off-by: Jad El Hajj * Test coverage Signed-off-by: Jad El Hajj --------- Signed-off-by: Jad El Hajj --- .../behavior_tree_engine.hpp | 17 ++ .../nav2_behavior_tree/bt_action_server.hpp | 30 ++- .../bt_action_server_impl.hpp | 66 +++--- .../src/behavior_tree_engine.cpp | 46 ++++ .../behavior_trees/follow_point.xml | 4 +- ...replanning_and_if_path_becomes_invalid.xml | 4 +- .../navigate_on_route_graph_w_recovery.xml | 4 +- ...hrough_poses_w_replanning_and_recovery.xml | 4 +- ...gate_to_pose_w_replanning_and_recovery.xml | 4 +- ..._replanning_goal_patience_and_recovery.xml | 4 +- ...eplanning_only_if_path_becomes_invalid.xml | 4 +- .../navigate_w_replanning_distance.xml | 4 +- ...e_w_replanning_only_if_goal_is_updated.xml | 4 +- ...eplanning_only_if_path_becomes_invalid.xml | 4 +- .../navigate_w_replanning_speed.xml | 4 +- .../navigate_w_replanning_time.xml | 4 +- ...global_planning_and_control_w_recovery.xml | 4 +- .../behavior_trees/odometry_calibration.xml | 4 +- .../src/navigators/navigate_through_poses.cpp | 9 +- .../src/navigators/navigate_to_pose.cpp | 9 +- .../behavior_tree/test_behavior_tree_node.cpp | 218 ++++++++++++++++-- ...nav_through_poses_tester_error_msg_node.py | 7 +- 22 files changed, 354 insertions(+), 104 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index b17a914a541..2371975317c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -86,6 +86,23 @@ class BehaviorTreeEngine const std::string & file_path, BT::Blackboard::Ptr blackboard); + /** + * @brief Extract BehaviorTree ID from BT file path or BT ID + * @param file_or_id + * @return std::string + */ + std::string extractBehaviorTreeID(const std::string & file_or_id); + + /** + * @brief Function to create a BT from a BehaviorTree ID + * @param tree_id BehaviorTree ID + * @param blackboard Blackboard for BT + * @return BT::Tree Created behavior tree + */ + BT::Tree createTree( + const std::string & tree_id, + BT::Blackboard::Ptr blackboard); + /** * @brief Add Groot2 monitor to publish BT status changes * @param tree BT to monitor 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 924d1ebc28a..aae15723e0f 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 @@ -99,12 +99,18 @@ class BtActionServer /** * @brief Replace current BT with another one - * @param bt_xml_filename The file containing the new BT, uses default filename if empty - * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false + * @param bt_xml_filename_or_id The file containing the new BT, uses default filename if empty or BT ID + * @return bool true if the resulting BT correspond to the one in bt_xml_filename_or_id. false * if something went wrong, and previous BT is maintained */ bool loadBehaviorTree( - const std::string & bt_xml_filename = ""); + const std::string & bt_xml_filename_or_id = ""); + + /** @brief Extract BehaviorTree ID from XML file + * @param filename The file containing the BT + * @return std::string BehaviorTree ID if found, empty string otherwise + */ + std::string extractBehaviorTreeID(const std::string & file_or_id); /** * @brief Getter function for BT Blackboard @@ -119,18 +125,18 @@ class BtActionServer * @brief Getter function for current BT XML filename * @return string Containing current BT XML filename */ - std::string getCurrentBTFilename() const + std::string getCurrentBTFilenameOrID() const { - return current_bt_xml_filename_; + return current_bt_file_or_id_; } /** - * @brief Getter function for default BT XML filename - * @return string Containing default BT XML filename + * @brief Getter function for default BT XML filename or ID + * @return string Containing default BT XML filename or ID */ - std::string getDefaultBTFilename() const + std::string getDefaultBTFilenameOrID() const { - return default_bt_xml_filename_; + return default_bt_xml_filename_or_id_; } /** @@ -245,8 +251,8 @@ class BtActionServer BT::Blackboard::Ptr blackboard_; // The XML file that contains the Behavior Tree to create - std::string current_bt_xml_filename_; - std::string default_bt_xml_filename_; + std::string current_bt_file_or_id_; + std::string default_bt_xml_filename_or_id_; std::vector search_directories_; // The wrapper class for the BT functionality @@ -283,7 +289,7 @@ class BtActionServer 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; + bool always_reload_bt_ = false; // Parameters for Groot2 monitoring bool enable_groot_monitoring_ = false; 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 10a8289d5f4..f229ede1fd7 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 @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -45,7 +46,7 @@ BtActionServer::BtActionServer( OnCompletionCallback on_completion_callback, const std::vector & search_directories) : action_name_(action_name), - default_bt_xml_filename_(default_bt_xml_filename), + default_bt_xml_filename_or_id_(default_bt_xml_filename), search_directories_(search_directories), plugin_lib_names_(plugin_lib_names), node_(parent), @@ -178,7 +179,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_); + node->get_parameter("always_reload_bt_xml", always_reload_bt_); // Get error code id names to grab off of the blackboard error_code_name_prefixes_ = node->get_parameter("error_code_name_prefixes").as_string_array(); @@ -204,8 +205,8 @@ template bool BtActionServer::on_activate() { resetInternalError(); - if (!loadBehaviorTree(default_bt_xml_filename_)) { - RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + if (!loadBehaviorTree(default_bt_xml_filename_or_id_)) { + RCLCPP_ERROR(logger_, "Error loading BT: %s", default_bt_xml_filename_or_id_.c_str()); return false; } action_server_->activate(); @@ -228,7 +229,7 @@ bool BtActionServer::on_cleanup() action_server_.reset(); topic_logger_.reset(); plugin_lib_names_.clear(); - current_bt_xml_filename_.clear(); + current_bt_file_or_id_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_); bt_->resetGrootMonitor(); @@ -246,40 +247,49 @@ void BtActionServer::setGrootMonitoring( } template -bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) +bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename_or_id) { namespace fs = std::filesystem; - // Empty filename is default for backward compatibility - auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; + // Empty argument is default for backward compatibility + auto file_or_id = + bt_xml_filename_or_id.empty() ? default_bt_xml_filename_or_id_ : bt_xml_filename_or_id; // 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"); + if (!always_reload_bt_ && current_bt_file_or_id_ == file_or_id) { + RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml or ID is already loaded"); return true; } // Reset any existing Groot2 monitoring bt_->resetGrootMonitor(); - std::ifstream xml_file(filename); - if (!xml_file.good()) { - setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - "Couldn't open BT XML file: " + filename); - return false; + bool is_bt_id = false; + if ((file_or_id.length() < 4) || + file_or_id.substr(file_or_id.length() - 4) != ".xml") + { + is_bt_id = true; } - const auto canonical_main_bt = fs::canonical(filename); - - // Register all XML behavior Subtrees found in the given directories + std::unordered_set used_bt_id; for (const auto & directory : search_directories_) { try { for (const auto & entry : fs::directory_iterator(directory)) { if (entry.path().extension() == ".xml") { - // Skip registering the main tree file - if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) { + auto current_bt_id = bt_->extractBehaviorTreeID(entry.path().string()); + if (current_bt_id.empty()) { + RCLCPP_ERROR(logger_, "Skipping BT file %s (missing ID)", + entry.path().string().c_str()); continue; } + auto [it, inserted] = used_bt_id.insert(current_bt_id); + if (!inserted) { + RCLCPP_WARN( + logger_, + "Warning: Duplicate BT IDs found. Make sure to have all BT IDs unique! " + "ID: %s File: %s", + current_bt_id.c_str(), entry.path().string().c_str()); + } bt_->registerTreeFromFile(entry.path().string()); } } @@ -289,18 +299,21 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml return false; } } - - // Try to load the main BT tree + // Try to load the main BT tree (by ID) try { - tree_ = bt_->createTreeFromFile(filename, blackboard_); + if(!is_bt_id) { + tree_ = bt_->createTreeFromFile(file_or_id, blackboard_); + } else { + tree_ = bt_->createTree(file_or_id, blackboard_); + } + for (auto & subtree : tree_.subtrees) { auto & blackboard = subtree->blackboard; blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); blackboard->set( - "wait_for_service_timeout", - wait_for_service_timeout_); + "wait_for_service_timeout", wait_for_service_timeout_); } } catch (const std::exception & e) { setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, @@ -310,8 +323,7 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml // Optional logging and monitoring topic_logger_ = std::make_unique(client_node_, tree_); - - current_bt_xml_filename_ = filename; + current_bt_file_or_id_ = file_or_id; if (enable_groot_monitoring_) { bt_->addGrootMonitoring(&tree_, groot_server_port_); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index a2b93f7d086..68bb2676b99 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -18,6 +18,7 @@ #include #include #include +#include "tinyxml2.h" //NOLINT #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/json_export.h" @@ -102,6 +103,51 @@ BehaviorTreeEngine::createTreeFromFile( return factory_.createTreeFromFile(file_path, blackboard); } +std::string BehaviorTreeEngine::extractBehaviorTreeID( + const std::string & bt_file) +{ + if(bt_file.empty()) { + RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), + "Error: Empty BT file passed to extractBehaviorTreeID"); + return ""; + } + tinyxml2::XMLDocument doc; + if (doc.LoadFile(bt_file.c_str()) != tinyxml2::XML_SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Error: Could not open or parse file %s", + bt_file.c_str()); + return ""; + } + tinyxml2::XMLElement * rootElement = doc.RootElement(); + if (!rootElement) { + RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Error: Root element not found in %s", + bt_file.c_str()); + return ""; + } + tinyxml2::XMLElement * btElement = rootElement->FirstChildElement("BehaviorTree"); + if (!btElement) { + RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), + "Error: element not found in %s", bt_file.c_str()); + return ""; + } + const char * idValue = btElement->Attribute("ID"); + if (idValue) { + return std::string(idValue); + } else { + RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), + "Error: ID attribute not found on element in %s", + bt_file.c_str()); + return ""; + } +} + +BT::Tree +BehaviorTreeEngine::createTree( + const std::string & tree_id, + BT::Blackboard::Ptr blackboard) +{ + return factory_.createTree(tree_id, blackboard); +} + /// @brief Register a tree from an XML file and return the tree void BehaviorTreeEngine::registerTreeFromFile( const std::string & file_path) diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ec16bf63051..5d5310f79cd 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -2,8 +2,8 @@ This Behavior Tree follows a dynamic pose to a certain distance --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index bffa6cee993..f2b6589d927 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -3,8 +3,8 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml index 7df6d6e0cb6..44662cb4656 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml @@ -10,8 +10,8 @@ It also has recovery actions specific to planning / control as well as general system issues. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 6c32906f4f9..cb88cbb3eed 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -3,8 +3,8 @@ This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously and it also has recovery actions specific to planning / control as well as general system issues. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index fdc96c7f62a..3fb6e4e5ff7 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -4,8 +4,8 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 70b9a930f5e..f79984b750e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -5,8 +5,8 @@ make the robot wait for a specific time, to see if the obstacle clears out before navigating along a significantly longer path to reach the goal location. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index 23114197a16..c13b2629dea 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -4,8 +4,8 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index cba0d5957c4..fa39ff05283 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path after every 1m. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 9ffc88b699f..f64063bbdcb 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path only when the goal is updated. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index 11b1050a936..1607057c0e2 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -1,8 +1,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 2fa10836816..9c9342e8bbd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path periodically proprortional to speed. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 161fb29af0c..2e43016775d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -2,8 +2,8 @@ This Behavior Tree replans the global path periodically at 1 Hz. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml index c25970af506..4692c9e17b0 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -13,8 +13,8 @@ It also has recovery actions specific to planning / control as well as general system issues. --> - - + + diff --git a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml index e66dded8745..a7f8760a336 100644 --- a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -2,8 +2,8 @@ his Behavior Tree drives in a square for odometry calibration experiments --> - - + + diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 9b8435ac4a3..92b6dfb7320 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -72,10 +72,9 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath( bool NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { - auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { + if (!bt_action_server_->loadBehaviorTree(goal->behavior_tree)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - "Error loading XML file: " + bt_xml_filename + ". Navigation canceled."); + "Error loading BT: " + goal->behavior_tree + ". Navigation canceled."); return false; } @@ -206,9 +205,9 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) { RCLCPP_INFO(logger_, "Received goal preemption request"); - if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() || + if (goal->behavior_tree == bt_action_server_->getCurrentBTFilenameOrID() || (goal->behavior_tree.empty() && - bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename())) + bt_action_server_->getCurrentBTFilenameOrID() == bt_action_server_->getDefaultBTFilenameOrID())) { // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index a9495645858..d4643a09ac8 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -82,10 +82,9 @@ NavigateToPoseNavigator::cleanup() bool NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) { - auto bt_xml_filename = goal->behavior_tree; - if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { + if (!bt_action_server_->loadBehaviorTree(goal->behavior_tree)) { bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, - std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled."); + std::string("Error loading BT: ") + goal->behavior_tree + ". Navigation canceled."); return false; } @@ -186,9 +185,9 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) { RCLCPP_INFO(logger_, "Received goal preemption request"); - if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() || + if (goal->behavior_tree == bt_action_server_->getCurrentBTFilenameOrID() || (goal->behavior_tree.empty() && - bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename())) + bt_action_server_->getCurrentBTFilenameOrID() == bt_action_server_->getDefaultBTFilenameOrID())) { // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 0c23468dc45..9a5f9a1826f 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. +#include +#include +#include #include #include #include @@ -19,6 +22,8 @@ #include #include #include +#include +#include "tinyxml2.h" //NOLINT #include "gtest/gtest.h" @@ -35,6 +40,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_behavior_tree/plugins_list.hpp" +#include "nav2_behavior_tree/behavior_tree_engine.hpp" #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -65,6 +71,7 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); nav2_util::Tokens plugin_libs = nav2_util::split(nav2::details::BT_BUILTIN_PLUGINS, ';'); + bt_engine_ = std::make_shared(plugin_libs, node_); for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); @@ -93,39 +100,41 @@ class BehaviorTreeHandler return blackboard; } - bool behaviorTreeFileValidation( - const std::string & filename) + std::string extractBehaviorTreeID(const std::string & file_or_id) { - std::ifstream xml_file(filename); - if (!xml_file.good()) { - RCLCPP_ERROR(node_->get_logger(), - "Couldn't open BT XML file: %s", filename.c_str()); - return false; - } - return true; + return bt_engine_->extractBehaviorTreeID(file_or_id); } - bool loadBehaviorTree( - const std::string & filename, + const std::string & file_or_id, const std::vector & search_directories) { - if (!behaviorTreeFileValidation(filename)) { - return false; - } - namespace fs = std::filesystem; - const auto canonical_main_bt = fs::canonical(filename); - - // Register all XML behavior Subtrees found in the given directories + bool is_bt_id = false; + if ((file_or_id.length() < 4) || + file_or_id.substr(file_or_id.length() - 4) != ".xml") + { + is_bt_id = true; + } + // Register all XML behavior subtrees in the directories + std::unordered_set used_bt_id; for (const auto & directory : search_directories) { try { for (const auto & entry : fs::directory_iterator(directory)) { if (entry.path().extension() == ".xml") { - // Skip registering the main tree file - if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) { + auto current_bt_id = bt_engine_->extractBehaviorTreeID(entry.path().string()); + if (current_bt_id.empty()) { + std::cerr << "[behavior_tree_handler]: Skipping BT file " + << entry.path().string() << " (missing ID)\n"; continue; } + auto [it, inserted] = used_bt_id.insert(current_bt_id); + if (!inserted) { + std::cout << "[behavior_tree_handler]: Warning: Duplicate BT IDs found. " + "Make sure to have all BT IDs unique! " + << "ID: " << current_bt_id + << " File: " << entry.path().string() << "\n"; + } factory_.registerBehaviorTreeFromFile(entry.path().string()); } } @@ -139,12 +148,18 @@ class BehaviorTreeHandler // Create and populate the blackboard blackboard = setBlackboardVariables(); - // Build the tree from the XML string + // Build the tree from the ID (resolved from or ) try { - tree = factory_.createTreeFromFile(filename, blackboard); + if(!is_bt_id) { + tree = factory_.createTreeFromFile(file_or_id, blackboard); + RCLCPP_WARN(node_->get_logger(), + "Loading BT using file path. This is deprecated. Please use the BT ID instead."); + } else { + tree = factory_.createTree(file_or_id, blackboard); + } } catch (BT::RuntimeError & exp) { - RCLCPP_ERROR(node_->get_logger(), "Failed to create BT from %s: %s", filename.c_str(), - exp.what()); + RCLCPP_ERROR(node_->get_logger(), + "Failed to create BT %s: %s", file_or_id.c_str(), exp.what()); return false; } @@ -175,6 +190,8 @@ class BehaviorTreeHandler std::shared_ptr odom_smoother_; BT::BehaviorTreeFactory factory_; + + std::shared_ptr bt_engine_; }; class BehaviorTreeTestFixture : public ::testing::Test @@ -234,7 +251,6 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) for (auto const & entry : std::filesystem::recursive_directory_iterator(root_dir)) { if (entry.is_regular_file() && entry.path().extension() == ".xml") { std::string main_bt = entry.path().string(); - std::cout << "Testing BT file: " << main_bt << std::endl; EXPECT_TRUE(bt_handler->loadBehaviorTree(main_bt, search_directories)) << "Failed to load: " << main_bt; @@ -290,6 +306,158 @@ TEST_F(BehaviorTreeTestFixture, TestWrongBTFormatXML) std::remove(malformed_main.c_str()); } +TEST_F(BehaviorTreeTestFixture, TestExtractBehaviorTreeID) +{ + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + // 1. Empty string input triggers "Empty file branch + auto empty_id = bt_handler->extractBehaviorTreeID(""); + EXPECT_TRUE(empty_id.empty()); + + // 2. Valid XML with ID + std::string valid_xml = "/tmp/extract_bt_id_valid.xml"; + write_file(valid_xml, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + auto id = bt_handler->extractBehaviorTreeID(valid_xml); + EXPECT_FALSE(id.empty()); + EXPECT_EQ(id, "TestTree"); + + // 3. Malformed XML (parser error) + std::string malformed_xml = "/tmp/extract_bt_id_malformed.xml"; + write_file(malformed_xml, ""); + auto missing_id = bt_handler->extractBehaviorTreeID(malformed_xml); + EXPECT_TRUE(missing_id.empty()); + + // 4. File does not exist + auto not_found = bt_handler->extractBehaviorTreeID("/tmp/does_not_exist.xml"); + EXPECT_TRUE(not_found.empty()); + + // 6. No root element + std::string no_root_file = "/tmp/extract_bt_id_no_root.xml"; + write_file(no_root_file, + "\n" + "\n"); + auto no_root_id = bt_handler->extractBehaviorTreeID(no_root_file); + EXPECT_TRUE(no_root_id.empty()); + + // 7. No child + std::string no_bt_element = "/tmp/extract_bt_id_no_bt.xml"; + write_file(no_bt_element, + "\n" + "\n" + " \n" + "\n"); + auto no_bt_id = bt_handler->extractBehaviorTreeID(no_bt_element); + EXPECT_TRUE(no_bt_id.empty()); + + // 8. No ID attribute + std::string no_id_attr = "/tmp/extract_bt_id_no_id.xml"; + write_file(no_id_attr, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + auto no_id = bt_handler->extractBehaviorTreeID(no_id_attr); + EXPECT_TRUE(no_id.empty()); + + // Cleanup + std::remove(valid_xml.c_str()); + std::remove(malformed_xml.c_str()); + std::remove(no_root_file.c_str()); + std::remove(no_bt_element.c_str()); + std::remove(no_id_attr.c_str()); +} + +TEST_F(BehaviorTreeTestFixture, TestLoadBehaviorTreeMissingAndDuplicateIDs) +{ + auto write_file = [](const std::string & path, const std::string & content) { + std::ofstream ofs(path); + ofs << content; + }; + + std::string tmp_dir = "/tmp/bt_test_dir"; + std::filesystem::create_directories(tmp_dir); + + // 1. File with missing ID (should be skipped) + std::string missing_id_file = tmp_dir + "/missing_id.xml"; + write_file(missing_id_file, + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"); + + // 2. Two files with the same ID (should trigger duplicate warning) + std::string dup1_file = tmp_dir + "/dup1.xml"; + std::string dup2_file = tmp_dir + "/dup2.xml"; + std::string dup_bt_content = + "\n" + "\n" + " \n" + " \n" + " \n" + "\n"; + write_file(dup1_file, dup_bt_content); + write_file(dup2_file, dup_bt_content); + + // Redirect cout and cerr to a stringstream + std::stringstream captured_output; + std::streambuf * old_cout_buf = std::cout.rdbuf(); + std::streambuf * old_cerr_buf = std::cerr.rdbuf(); + + std::cout.rdbuf(captured_output.rdbuf()); + std::cerr.rdbuf(captured_output.rdbuf()); + + std::vector search_dirs = {tmp_dir}; + bool result = bt_handler->loadBehaviorTree("DuplicateTree", search_dirs); + + // Restore cout and cerr + std::cout.rdbuf(old_cout_buf); + std::cerr.rdbuf(old_cerr_buf); + + // Check the captured output for the expected log messages + std::string log_output = captured_output.str(); + + // Assert that the function returned true + EXPECT_TRUE(result); + + // Assert that the error log for the missing ID was found + EXPECT_NE(log_output.find("[behavior_tree_handler]: Skipping BT file " + missing_id_file + + " (missing ID)"), std::string::npos); + + // Assert that the warning log for the duplicate ID was found + EXPECT_NE( + log_output.find( + "Warning: Duplicate BT IDs found. Make sure to have all BT IDs unique! " + "ID: DuplicateTree File: "), std::string::npos); + + // Cleanup + std::filesystem::remove_all(tmp_dir); +} + +TEST_F(BehaviorTreeTestFixture, TestLoadByIdInsteadOfFile) +{ + const auto root_dir = std::filesystem::path( + ament_index_cpp::get_package_share_directory("nav2_bt_navigator") + ) / "behavior_trees"; + std::vector search_directories = {root_dir.string()}; + + // Load by BT ID instead of file + EXPECT_TRUE(bt_handler->loadBehaviorTree("NavigateToPoseWReplanningAndRecovery", + search_directories)); +} + /** * Test scenario: * diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py index 4cd1003d068..ba11a3e1ac3 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py @@ -283,8 +283,11 @@ def run_all_tests(robot_tester: NavTester) -> bool: goal_pose=pose_out_of_bounds, behavior_tree='behavior_tree_that_does_not_exist.xml', expected_error_code=NavigateThroughPoses.Result.FAILED_TO_LOAD_BEHAVIOR_TREE, - expected_error_msg=('Error loading XML file: behavior_tree_that_does_not_exist.xml. ' - 'Navigation canceled.')) + expected_error_msg=( + 'Error loading BT: behavior_tree_that_does_not_exist.xml. ' + 'Navigation canceled.' + ), + ) if result: robot_tester.info_msg('Test goal out of bounds') From 86477084309e18fa9989eaf3f01f1a503f147118 Mon Sep 17 00:00:00 2001 From: Koensgen Benjamin Date: Thu, 18 Sep 2025 16:53:18 +0200 Subject: [PATCH 02/16] tests(opennav_docking): restore executor-based spinning (#5533) Signed-off-by: bkoensgen --- .../test/test_simple_charging_dock.cpp | 43 +++++++++++++------ .../test/test_simple_non_charging_dock.cpp | 36 +++++++++++----- 2 files changed, 54 insertions(+), 25 deletions(-) diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index a95bd36eda3..ee27bc0d33e 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -86,6 +86,8 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -95,7 +97,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); @@ -106,7 +108,7 @@ TEST(SimpleChargingDockTests, BatteryState) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(dock->isCharging()); EXPECT_FALSE(dock->hasStoppedCharging()); @@ -139,6 +141,8 @@ TEST(SimpleChargingDockTests, StallDetection) rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names))); dock->configure(node, "my_dock", nullptr); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); EXPECT_EQ(dock->getStallJointNames(), names); // Stopped, but below effort threshold @@ -149,7 +153,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -161,7 +165,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -173,7 +177,7 @@ TEST(SimpleChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(dock->isDocked()); @@ -245,6 +249,8 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped pose; @@ -258,7 +264,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; @@ -287,6 +293,8 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -294,7 +302,7 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -325,6 +333,8 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -347,7 +357,7 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -452,12 +462,14 @@ TEST(SimpleChargingDockTests, DetectorLifecycle) dock->activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Spin to process async service call auto start_time = std::chrono::steady_clock::now(); while (!service_called && std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(service_called); @@ -502,11 +514,13 @@ TEST(SimpleChargingDockTests, SubscriptionCallback) publisher->on_activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Wait for the publisher and subscriber to connect. int tries = 0; while (publisher->get_subscription_count() == 0 && tries++ < 10) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); } ASSERT_GT(publisher->get_subscription_count(), 0); @@ -514,7 +528,7 @@ TEST(SimpleChargingDockTests, SubscriptionCallback) // Publish a message to trigger the subscription callback. publisher->publish(geometry_msgs::msg::PoseStamped{}); std::this_thread::sleep_for(50ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Verify the detector state was updated, proving the callback was executed. EXPECT_TRUE(dock->isDetectorActive()); @@ -608,17 +622,18 @@ TEST(SimpleChargingDockTests, SubscriptionPersistent) auto publisher = node->create_publisher( "detected_dock_pose", rclcpp::QoS(1)); publisher->on_activate(); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); int tries = 0; while (publisher->get_subscription_count() == 0 && tries++ < 10) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); } ASSERT_GT(publisher->get_subscription_count(), 0); publisher->publish(geometry_msgs::msg::PoseStamped{}); std::this_thread::sleep_for(50ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Verify the detector state changed, proving the callback was executed. EXPECT_TRUE(dock->isDetectorActive()); diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 77951bd6cd5..6e3d01c4428 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -99,6 +99,8 @@ TEST(SimpleNonChargingDockTests, StallDetection) EXPECT_EQ(dock->getStallJointNames(), names); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -110,7 +112,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg); rclcpp::Rate r(2); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -122,7 +124,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg2); rclcpp::Rate r1(2); r1.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_FALSE(dock->isDocked()); @@ -134,7 +136,7 @@ TEST(SimpleNonChargingDockTests, StallDetection) pub->publish(msg3); rclcpp::Rate r2(2); r2.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(dock->isDocked()); @@ -206,6 +208,8 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped pose; @@ -219,7 +223,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) detected_pose.pose.position.x = 0.1; detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; @@ -248,6 +252,8 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) dock->configure(node, "my_dock", tf_buffer); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); geometry_msgs::msg::PoseStamped detected_pose; detected_pose.header.stamp = node->now(); @@ -255,7 +261,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -286,6 +292,8 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -308,7 +316,7 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) detected_pose.pose.position.x = 1.0; detected_pose.pose.position.y = 1.0; pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -413,12 +421,14 @@ TEST(SimpleNonChargingDockTests, DetectorLifecycle) dock->activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Spin to process async service call auto start_time = std::chrono::steady_clock::now(); while (!service_called && std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(service_called); @@ -464,11 +474,13 @@ TEST(SimpleNonChargingDockTests, SubscriptionCallback) publisher->on_activate(); dock->startDetectionProcess(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Wait for the publisher and subscriber to connect. int tries = 0; while (publisher->get_subscription_count() == 0 && tries++ < 10) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); } ASSERT_GT(publisher->get_subscription_count(), 0); @@ -476,7 +488,7 @@ TEST(SimpleNonChargingDockTests, SubscriptionCallback) // Publish a message to trigger the subscription callback. publisher->publish(geometry_msgs::msg::PoseStamped{}); std::this_thread::sleep_for(50ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Verify the detector state was updated, proving the callback was executed. EXPECT_TRUE(dock->isDetectorActive()); @@ -567,6 +579,8 @@ TEST(SimpleNonChargingDockTests, SubscriptionPersistent) auto dock = std::make_unique(); dock->configure(node, "my_dock", nullptr); dock->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // The subscription should be active immediately after configuration. auto publisher = node->create_publisher( @@ -575,14 +589,14 @@ TEST(SimpleNonChargingDockTests, SubscriptionPersistent) int tries = 0; while (publisher->get_subscription_count() == 0 && tries++ < 10) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); } ASSERT_GT(publisher->get_subscription_count(), 0); publisher->publish(geometry_msgs::msg::PoseStamped{}); std::this_thread::sleep_for(50ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Verify the detector state changed, proving the callback was executed. EXPECT_TRUE(dock->isDetectorActive()); From 547820ff76255c85a1ac3a60b5cf58ca80cd96cd Mon Sep 17 00:00:00 2001 From: Jonas Otto Date: Thu, 18 Sep 2025 17:37:36 +0200 Subject: [PATCH 03/16] add bt nodes GetCurrentPose, ConcatenatePaths, WouldARouteRecoveryHelp, ArePosesNear to nav2_tree_nodes.xml (#5534) Signed-off-by: Jonas Otto --- nav2_behavior_tree/nav2_tree_nodes.xml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 230ce02f70c..eabba3a4b09 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -311,6 +311,18 @@ Stamped extracted pose + + Global reference frame + robot base frame + Current pose output + + + + Input Path 1 to cancatenate + Input Path 2 to cancatenate + Paths concatenated + + Destination @@ -388,11 +400,21 @@ Error code + + Error code + + Error code Error codes to check, user defined + + Destination + Destination + Global frame + Tolerance + From c95673edb8be7f38ed2e98f74491c47858d1a82f Mon Sep 17 00:00:00 2001 From: Abhishekh Reddy Date: Fri, 19 Sep 2025 17:14:30 -0400 Subject: [PATCH 04/16] Add a service for enabling/disabling the collision monitor (#5493) * Added std_srvs package to dependencies Signed-off-by: Abhishekh Reddy * Declared service and callback for enabling/disabling collision monitor Signed-off-by: Abhishekh Reddy * Declared a variable to store collision monitor enable/disable state Signed-off-by: Abhishekh Reddy * Added initialization for collision monitor enable/disable service Signed-off-by: Abhishekh Reddy * Implemented service callback for collision monitor enable/disable service Signed-off-by: Abhishekh Reddy * Removed std_srvs package dependency Signed-off-by: Abhishekh Reddy * Added Toggle interface Signed-off-by: Abhishekh Reddy * Replaced Trigger interface with the new Toggle interface Signed-off-by: Abhishekh Reddy * Added default initialization for enabled flag Signed-off-by: Abhishekh Reddy * Fixed toggle service name Signed-off-by: Abhishekh Reddy * Updated toggle logic for collision monitor Signed-off-by: Abhishekh Reddy * Added a new line at the end of file Signed-off-by: Abhishekh Reddy * Update nav2_collision_monitor/src/collision_monitor_node.cpp Co-authored-by: Steve Macenski Signed-off-by: Abhishekh Reddy * Update nav2_collision_monitor/src/collision_monitor_node.cpp Co-authored-by: Steve Macenski Signed-off-by: Abhishekh Reddy * Added enabled check for logging Signed-off-by: Abhishekh Reddy * Added a unit test for toggle service Signed-off-by: Abhishekh Reddy * Made the getter const and added a comment Signed-off-by: Abhishekh Reddy * Replaced rclcpp::spin_some Signed-off-by: Abhishekh Reddy --------- Signed-off-by: Abhishekh Reddy Co-authored-by: Steve Macenski --- .../collision_monitor_node.hpp | 17 +++++ .../src/collision_monitor_node.cpp | 29 +++++++-- .../test/collision_monitor_node_test.cpp | 62 +++++++++++++++++++ nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/srv/Toggle.srv | 4 ++ 5 files changed, 109 insertions(+), 4 deletions(-) create mode 100644 nav2_msgs/srv/Toggle.srv diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index c53540089d7..85a04288087 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -33,6 +33,7 @@ #include "nav2_util/twist_publisher.hpp" #include "nav2_util/twist_subscriber.hpp" #include "nav2_msgs/msg/collision_monitor_state.hpp" +#include "nav2_msgs/srv/toggle.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" @@ -200,6 +201,16 @@ class CollisionMonitor : public nav2::LifecycleNode */ void publishPolygons() const; + /** + * @brief Enable/disable collision monitor service callback + * @param request Service request + * @param response Service response + */ + void toggleCMServiceCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + // ----- Variables ----- /// @brief TF buffer @@ -227,6 +238,12 @@ class CollisionMonitor : public nav2::LifecycleNode nav2::Publisher::SharedPtr collision_points_marker_pub_; + /// @brief Enable/disable collision monitor service + nav2::ServiceServer::SharedPtr toggle_cm_service_; + + /// @brief Whether collision monitor is enabled + bool enabled_; + /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index d7ec6131220..cf69674e66e 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -25,12 +25,14 @@ #include "nav2_collision_monitor/kinematics.hpp" +using namespace std::placeholders; + namespace nav2_collision_monitor { CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options) : nav2::LifecycleNode("collision_monitor", options), - process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""}, + enabled_{true}, process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""}, stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0) { } @@ -81,6 +83,11 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) collision_points_marker_pub_ = this->create_publisher( "~/collision_points_marker"); + // Toggle service initialization + toggle_cm_service_ = create_service( + "~/toggle", + std::bind(&CollisionMonitor::toggleCMServiceCallback, this, _1, _2, _3)); + nav2::declare_parameter_if_not_declared( node, "use_realtime_priority", rclcpp::ParameterValue(false)); bool use_realtime_priority = false; @@ -472,7 +479,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } for (std::shared_ptr polygon : polygons_) { - if (!polygon->getEnabled()) { + if (!polygon->getEnabled() || !enabled_) { continue; } if (robot_action.action_type == STOP) { @@ -499,7 +506,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } } - if (robot_action.polygon_name != robot_action_prev_.polygon_name) { + if ((robot_action.polygon_name != robot_action_prev_.polygon_name) && enabled_) { // Report changed robot behavior notifyActionState(robot_action, action_polygon); } @@ -652,12 +659,26 @@ void CollisionMonitor::notifyActionState( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - if (polygon->getEnabled()) { + if (polygon->getEnabled() || !enabled_) { polygon->publish(); } } } +void CollisionMonitor::toggleCMServiceCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + enabled_ = request->enable; + + std::stringstream message; + message << "Collision monitor toggled " << (enabled_ ? "on" : "off") << " successfully"; + + response->success = true; + response->message = message.str(); +} + } // namespace nav2_collision_monitor #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 92dd5e6751a..625d4cde007 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -131,6 +131,11 @@ class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor } return false; } + + bool isEnabled() const + { + return enabled_; + } }; // CollisionMonitorWrapper class Tester : public ::testing::Test @@ -180,6 +185,9 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout); bool waitActionState(const std::chrono::nanoseconds & timeout); bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); + bool waitToggle( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout); protected: void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); @@ -218,6 +226,9 @@ class Tester : public ::testing::Test // Service client for setting CollisionMonitor parameters nav2::ServiceClient::SharedPtr parameters_client_; + + // Service client for toggling collision monitor + nav2::ServiceClient::SharedPtr toggle_client_; }; // Tester Tester::Tester() @@ -261,6 +272,8 @@ Tester::Tester() cm_->create_client( std::string( cm_->get_name()) + "/set_parameters"); + + toggle_client_ = cm_->create_client("~/toggle"); } Tester::~Tester() @@ -763,6 +776,22 @@ bool Tester::waitFuture( return false; } +bool Tester::waitToggle( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return true; + } + executor_->spin_some(); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) { rclcpp::Time start_time = cm_->now(); @@ -804,6 +833,39 @@ void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray: collision_points_marker_msg_ = msg; } +TEST_F(Tester, testToggleService) +{ + // Set parameters for collision monitor + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start collision monitor node + cm_->start(); + + auto request = std::make_shared(); + + // Disable test + request->enable = false; + { + auto result_future = toggle_client_->async_call(request); + ASSERT_TRUE(waitToggle(result_future, 2s)); + } + ASSERT_FALSE(cm_->isEnabled()); + + // Enable test + request->enable = true; + { + auto result_future = toggle_client_->async_call(request); + ASSERT_TRUE(waitToggle(result_future, 2s)); + } + ASSERT_TRUE(cm_->isEnabled()); + + // Stop the collision monitor + cm_->stop(); +} + TEST_F(Tester, testProcessStopSlowdownLimit) { rclcpp::Time curr_time = cm_->now(); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 41311173486..b44e08ccb3a 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -60,6 +60,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/GetShapes.srv" "srv/SetRouteGraph.srv" "srv/DynamicEdges.srv" + "srv/Toggle.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" diff --git a/nav2_msgs/srv/Toggle.srv b/nav2_msgs/srv/Toggle.srv new file mode 100644 index 00000000000..c4ed478ca6b --- /dev/null +++ b/nav2_msgs/srv/Toggle.srv @@ -0,0 +1,4 @@ +bool enable +--- +bool success +string message From 16e63a46c0e25339dfddee468589b456cf0af7df Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 24 Sep 2025 23:59:22 +0800 Subject: [PATCH 05/16] Refactor common functions in smoother and planner (#5537) * Refactor common functions in planner and smoother Signed-off-by: mini-1235 * Add unit tests for smoother utils Signed-off-by: Maurice * Update cmakelist and linting Signed-off-by: mini-1235 * Update nav2_util/include/nav2_util/smoother_utils.hpp Co-authored-by: Steve Macenski Signed-off-by: mini-1235 * Update nav2_util/include/nav2_util/smoother_utils.hpp Co-authored-by: Steve Macenski Signed-off-by: mini-1235 * Update unit tests Signed-off-by: Maurice --------- Signed-off-by: mini-1235 Signed-off-by: Maurice Co-authored-by: Steve Macenski --- .../include/nav2_smac_planner/smoother.hpp | 27 ----- nav2_smac_planner/src/smoother.cpp | 99 ++--------------- nav2_smac_planner/test/test_smoother.cpp | 9 -- .../nav2_smoother/savitzky_golay_smoother.hpp | 2 +- .../include/nav2_smoother/simple_smoother.hpp | 2 +- nav2_smoother/src/savitzky_golay_smoother.cpp | 7 +- nav2_smoother/src/simple_smoother.cpp | 14 +-- .../test/test_savitzky_golay_smoother.cpp | 1 - nav2_smoother/test/test_simple_smoother.cpp | 6 -- nav2_util/CMakeLists.txt | 2 + .../include/nav2_util}/smoother_utils.hpp | 51 +++++---- nav2_util/package.xml | 1 + nav2_util/test/CMakeLists.txt | 3 + nav2_util/test/test_smoother_utils.cpp | 100 ++++++++++++++++++ 14 files changed, 159 insertions(+), 165 deletions(-) rename {nav2_smoother/include/nav2_smoother => nav2_util/include/nav2_util}/smoother_utils.hpp (71%) create mode 100644 nav2_util/test/test_smoother_utils.cpp diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index 00503d4d293..b4a65be8e48 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -27,16 +27,6 @@ namespace nav2_smac_planner { -/** - * @class nav2_smac_planner::PathSegment - * @brief A segment of a path in start/end indices - */ -struct PathSegment -{ - unsigned int start; - unsigned int end; -}; - /** * @struct nav2_smac_planner::BoundaryPoints * @brief Set of boundary condition points from expansion @@ -144,14 +134,6 @@ class Smoother geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, const double & value); - /** - * @brief Finds the starting and end indices of path segments where - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param path Path in which to look for cusps - * @return Set of index pairs for each segment of the path in a given direction - */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); - /** * @brief Enforced minimum curvature boundary conditions on plan output * the robot is traveling in the same direction (e.g. forward vs reverse) @@ -214,15 +196,6 @@ class Smoother template BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end); - /** - * @brief For a given path, update the path point orientations based on smoothing - * @param path Path to approximate the path orientation in - * @param reversing_segment Return if this is a reversing segment - */ - inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment); - double min_turning_rad_, tolerance_, data_w_, smooth_w_; int max_its_, refinement_ctr_, refinement_num_; bool is_holonomic_, do_refinement_; diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index be417fa1be7..456f26a2566 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -24,11 +24,13 @@ #include "tf2/utils.hpp" #include "nav2_smac_planner/smoother.hpp" +#include "nav2_util/smoother_utils.hpp" namespace nav2_smac_planner { using namespace nav2_util::geometry_utils; // NOLINT using namespace std::chrono; // NOLINT +using nav2_util::PathSegment; Smoother::Smoother(const SmootherParams & params) { @@ -62,7 +64,8 @@ bool Smoother::smooth( bool success = true, reversing_segment; nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; - std::vector path_segments = findDirectionalPathSegments(path); + std::vector path_segments = nav2_util::findDirectionalPathSegments(path, + is_holonomic_); for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 10) { @@ -130,7 +133,7 @@ bool Smoother::smoothImpl( rclcpp::get_logger("SmacPlannerSmoother"), "Number of iterations has exceeded limit of %i.", max_its_); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); + nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_); return false; } @@ -142,7 +145,7 @@ bool Smoother::smoothImpl( rclcpp::get_logger("SmacPlannerSmoother"), "Smoothing time exceeded allowed duration of %0.2f.", max_time); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); + nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_); return false; } @@ -176,7 +179,7 @@ bool Smoother::smoothImpl( "Smoothing process resulted in an infeasible collision. " "Returning the last path before the infeasibility was introduced."); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); + nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_); return false; } } @@ -191,7 +194,7 @@ bool Smoother::smoothImpl( smoothImpl(new_path, reversing_segment, costmap, max_time); } - updateApproximatePathOrientations(new_path, reversing_segment); + nav2_util::updateApproximatePathOrientations(new_path, reversing_segment, is_holonomic_); path = new_path; return true; } @@ -221,92 +224,6 @@ void Smoother::setFieldByDim( } } -std::vector Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path) -{ - std::vector segments; - PathSegment curr_segment; - curr_segment.start = 0; - - // If holonomic, no directional changes and - // may have abrupt angular changes from naive grid search - if (is_holonomic_) { - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; - } - - // Iterating through the path to determine the position of the cusp - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existence of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - - // Checking for the existence of a differential rotation in place. - double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); - double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); - double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); - if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - } - - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; -} - -void Smoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment) -{ - double dx, dy, theta, pt_yaw; - reversing_segment = false; - - // Find if this path segment is in reverse - dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; - dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; - theta = atan2(dy, dx); - pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (!is_holonomic_ && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { - reversing_segment = true; - } - - // Find the angle relative the path position vectors - for (unsigned int i = 0; i != path.poses.size() - 1; i++) { - dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; - dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; - theta = atan2(dy, dx); - - // If points are overlapping, pass - if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { - continue; - } - - // Flip the angle if this path segment is in reverse - if (reversing_segment) { - theta += M_PI; // orientationAroundZAxis will normalize - } - - path.poses[i].pose.orientation = orientationAroundZAxis(theta); - } -} - unsigned int Smoother::findShortestBoundaryExpansionIdx( const BoundaryExpansions & boundary_expansions) { diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 6c223176c23..2b38d854255 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -37,11 +37,6 @@ class SmootherWrapper : public nav2_smac_planner::Smoother explicit SmootherWrapper(const SmootherParams & params) : nav2_smac_planner::Smoother(params) {} - - std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) - { - return findDirectionalPathSegments(path); - } }; TEST(SmootherTest, test_full_smoother) @@ -133,10 +128,6 @@ TEST(SmootherTest, test_full_smoother) y_m = path[i].y; } - // Check that we accurately detect that this path has a reversing segment - auto path_segs = smoother->findDirectionalPathSegmentsWrapper(plan); - EXPECT_TRUE(path_segs.size() == 2u || path_segs.size() == 3u); - // Test smoother, should succeed with same number of points // and shorter overall length, while still being collision free. auto path_size_in = plan.poses.size(); diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp index 1f56e8440e5..9a20f19fa59 100644 --- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -25,7 +25,7 @@ #include #include "nav2_core/smoother.hpp" -#include "nav2_smoother/smoother_utils.hpp" +#include "nav2_util/smoother_utils.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index dfaeba953aa..566c5700b76 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -24,7 +24,7 @@ #include #include "nav2_core/smoother.hpp" -#include "nav2_smoother/smoother_utils.hpp" +#include "nav2_util/smoother_utils.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 90180734e5c..e38b5134ea4 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -19,11 +19,10 @@ namespace nav2_smoother { - -using namespace smoother_utils; // NOLINT using namespace nav2_util::geometry_utils; // NOLINT using namespace std::chrono; // NOLINT using nav2::declare_parameter_if_not_declared; +using nav2_util::PathSegment; void SavitzkyGolaySmoother::configure( const nav2::LifecycleNode::WeakPtr & parent, @@ -89,7 +88,7 @@ bool SavitzkyGolaySmoother::smooth( std::vector path_segments{ PathSegment{0u, static_cast(path.poses.size() - 1)}}; if (enforce_path_inversion_) { - path_segments = findDirectionalPathSegments(path); + path_segments = nav2_util::findDirectionalPathSegments(path); } // Minimum point size to smooth is SG filter size + start + end @@ -171,7 +170,7 @@ bool SavitzkyGolaySmoother::smoothImpl( } } - updateApproximatePathOrientations(path, reversing_segment); + nav2_util::updateApproximatePathOrientations(path, reversing_segment); return true; } diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 5179784f83a..bb93172f81c 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -19,10 +19,10 @@ namespace nav2_smoother { -using namespace smoother_utils; // NOLINT using namespace nav2_util::geometry_utils; // NOLINT using namespace std::chrono; // NOLINT using nav2::declare_parameter_if_not_declared; +using nav2_util::PathSegment; void SimpleSmoother::configure( const nav2::LifecycleNode::WeakPtr & parent, @@ -72,10 +72,10 @@ bool SimpleSmoother::smooth( nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; - std::vector path_segments{PathSegment{ + std::vector path_segments{PathSegment{ 0u, static_cast(path.poses.size() - 1)}}; if (enforce_path_inversion_) { - path_segments = findDirectionalPathSegments(path); + path_segments = nav2_util::findDirectionalPathSegments(path); } std::lock_guard lock(*(costmap->getMutex())); @@ -137,7 +137,7 @@ void SimpleSmoother::smoothImpl( logger_, "Number of iterations has exceeded limit of %i.", max_its_); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); + nav2_util::updateApproximatePathOrientations(path, reversing_segment); return; } @@ -149,7 +149,7 @@ void SimpleSmoother::smoothImpl( logger_, "Smoothing time exceeded allowed duration of %0.2f.", max_time); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); + nav2_util::updateApproximatePathOrientations(path, reversing_segment); throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration"); } @@ -183,7 +183,7 @@ void SimpleSmoother::smoothImpl( "Smoothing process resulted in an infeasible collision. " "Returning the last path before the infeasibility was introduced."); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); + nav2_util::updateApproximatePathOrientations(path, reversing_segment); return; } } @@ -198,7 +198,7 @@ void SimpleSmoother::smoothImpl( smoothImpl(new_path, reversing_segment, costmap, max_time); } - updateApproximatePathOrientations(new_path, reversing_segment); + nav2_util::updateApproximatePathOrientations(new_path, reversing_segment); path = new_path; } diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp index b31587a53f7..b1b59c3f2d8 100644 --- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -30,7 +30,6 @@ #include "nav2_smoother/savitzky_golay_smoother.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" -using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT using namespace std::chrono_literals; // NOLINT diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index 311b9838276..be1fadb2338 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -24,7 +24,6 @@ #include "nav2_smoother/simple_smoother.hpp" #include "nav2_core/smoother_exceptions.hpp" -using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT using namespace std::chrono_literals; // NOLINT @@ -36,11 +35,6 @@ class SmootherWrapper : public nav2_smoother::SimpleSmoother { } - std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) - { - return findDirectionalPathSegments(path); - } - void setMaxItsToInvalid() { max_its_ = 0; diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 2a2f4061818..f0c51b06905 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) +find_package(angles REQUIRED) nav2_package() @@ -61,6 +62,7 @@ ament_export_dependencies( tf2_ros pluginlib nav2_ros_common + angles ) ament_export_targets(${library_name}) diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_util/include/nav2_util/smoother_utils.hpp similarity index 71% rename from nav2_smoother/include/nav2_smoother/smoother_utils.hpp rename to nav2_util/include/nav2_util/smoother_utils.hpp index 500dd7a9351..979345ad7e4 100644 --- a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp +++ b/nav2_util/include/nav2_util/smoother_utils.hpp @@ -12,31 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ -#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ +#ifndef NAV2_UTIL__SMOOTHER_UTILS_HPP_ +#define NAV2_UTIL__SMOOTHER_UTILS_HPP_ #include #include #include -#include #include -#include #include -#include "nav2_core/smoother.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_ros_common/node_utils.hpp" #include "nav_msgs/msg/path.hpp" #include "angles/angles.h" #include "tf2/utils.hpp" -namespace smoother_utils +namespace nav2_util { /** - * @class nav2_smoother::PathSegment + * @class nav2_util::PathSegment * @brief A segment of a path in start/end indices */ struct PathSegment @@ -45,16 +39,29 @@ struct PathSegment unsigned int end; }; -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - +/** + * @brief Finds the starting and end indices of path segments where + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param path Path in which to look for cusps + * @param is_holonomic Whether the motion model is holonomic (default is false) + Only set as true when the input path is known to be generated from a holonomic planner like NavFn. + * @return Set of index pairs for each segment of the path in a given direction + */ inline std::vector findDirectionalPathSegments( - const nav_msgs::msg::Path & path) + const nav_msgs::msg::Path & path, bool is_holonomic = false) { std::vector segments; PathSegment curr_segment; curr_segment.start = 0; + // If holonomic, no directional changes and + // may have abrupt angular changes from naive grid search + if (is_holonomic) { + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; + } + // Iterating through the path to determine the position of the cusp for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { // We have two vectors for the dot product OA and AB. Determining the vectors. @@ -91,9 +98,17 @@ inline std::vector findDirectionalPathSegments( return segments; } +/** + * @brief For a given path, update the path point orientations based on smoothing + * @param path Path to approximate the path orientation in + * @param reversing_segment Return if this is a reversing segment + * @param is_holonomic Whether the motion model is holonomic (default is false) + Only set as true when the input path is known to be generated from a holonomic planner like NavFn. + */ inline void updateApproximatePathOrientations( nav_msgs::msg::Path & path, - bool & reversing_segment) + bool & reversing_segment, + bool is_holonomic = false) { double dx, dy, theta, pt_yaw; reversing_segment = false; @@ -103,7 +118,7 @@ inline void updateApproximatePathOrientations( dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; theta = atan2(dy, dx); pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + if (!is_holonomic && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { reversing_segment = true; } @@ -127,6 +142,6 @@ inline void updateApproximatePathOrientations( } } -} // namespace smoother_utils +} // namespace nav2_util -#endif // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ +#endif // NAV2_UTIL__SMOOTHER_UTILS_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 334879c3981..b8c490de287 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -32,6 +32,7 @@ tf2_ros pluginlib nav2_ros_common + angles ament_lint_common ament_lint_auto diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index c8447eaab6e..b7f290f8e78 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -21,6 +21,9 @@ target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS}) ament_add_gtest(test_controller_utils test_controller_utils.cpp) target_link_libraries(test_controller_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS}) +ament_add_gtest(test_smoother_utils test_smoother_utils.cpp) +target_link_libraries(test_smoother_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS} angles::angles) + ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) target_include_directories(test_base_footprint_publisher PRIVATE "$") diff --git a/nav2_util/test/test_smoother_utils.cpp b/nav2_util/test/test_smoother_utils.cpp new file mode 100644 index 00000000000..b5fdbbff0d9 --- /dev/null +++ b/nav2_util/test/test_smoother_utils.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include "nav2_util/smoother_utils.hpp" +#include "gtest/gtest.h" + +using nav2_util::findDirectionalPathSegments; +using nav2_util::updateApproximatePathOrientations; + +geometry_msgs::msg::PoseStamped makePose(double x, double y, double yaw = 0.0) +{ + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + return pose; +} + +TEST(SmootherUtils, HolonomicSingleSegment) +{ + nav_msgs::msg::Path path; + path.poses.push_back(makePose(0, 0)); + path.poses.push_back(makePose(1, 0)); + path.poses.push_back(makePose(2, 0)); + + auto segments = findDirectionalPathSegments(path, true); + ASSERT_EQ(segments.size(), 1u); + EXPECT_EQ(segments[0].start, 0u); + EXPECT_EQ(segments[0].end, 2u); +} + +TEST(SmootherUtils, ForwardAndReverseSegments) +{ + nav_msgs::msg::Path path; + path.poses.push_back(makePose(0, 0)); + path.poses.push_back(makePose(1, 0)); + path.poses.push_back(makePose(0, 0)); + path.poses.push_back(makePose(-1, 0)); + + auto segments = findDirectionalPathSegments(path, false); + ASSERT_GE(segments.size(), 2u); + EXPECT_EQ(segments[0].start, 0u); + EXPECT_LT(segments[0].end, path.poses.size()); +} + +TEST(SmootherUtils, RotationInPlaceCreatesSegment) +{ + nav_msgs::msg::Path path; + // Same position, but rotating + path.poses.push_back(makePose(0, 0, 0.0)); + path.poses.push_back(makePose(0, 0, 0.0)); + path.poses.push_back(makePose(0, 0, M_PI_2)); + path.poses.push_back(makePose(0, 0, M_PI)); + + auto segments = findDirectionalPathSegments(path, false); + ASSERT_GE(segments.size(), 2u); +} + +TEST(SmootherUtils, UpdateApproximatePathForward) +{ + nav_msgs::msg::Path path; + path.poses.push_back(makePose(0, 0, 0.0)); + path.poses.push_back(makePose(1, 0, 0.0)); + path.poses.push_back(makePose(2, 0, 0.0)); + + bool reversing = false; + updateApproximatePathOrientations(path, reversing, false); + + EXPECT_FALSE(reversing); + double yaw0 = tf2::getYaw(path.poses[0].pose.orientation); + EXPECT_NEAR(yaw0, 0.0, 1e-5); +} + +TEST(SmootherUtils, UpdateApproximatePathReverse) +{ + nav_msgs::msg::Path path; + path.poses.push_back(makePose(0, 0, 0)); + path.poses.push_back(makePose(-1, 0, 0)); + path.poses.push_back(makePose(-2, 0, 0)); + + bool reversing = false; + updateApproximatePathOrientations(path, reversing, false); + + EXPECT_TRUE(reversing); + double yaw0 = tf2::getYaw(path.poses[0].pose.orientation); + EXPECT_NEAR(yaw0, 0, 1e-5); +} From d36bdd19c25240924ce7563cc5f57efa3be12000 Mon Sep 17 00:00:00 2001 From: DavidG-Develop <147402604+DavidG-Develop@users.noreply.github.com> Date: Thu, 25 Sep 2025 23:04:26 +0200 Subject: [PATCH 06/16] Collision monitor toggle bt plugin (#5532) * add Bt pluging for toggle collision monitor service Signed-off-by: David G * Add test for btt plugin Signed-off-by: David G * Clean up test Signed-off-by: David G * Fix copyright in header Signed-off-by: David G * uncrustify Signed-off-by: David G * fix lint Signed-off-by: David G * fix circle ci Signed-off-by: David G --------- Signed-off-by: David G Signed-off-by: DavidG-Develop <147402604+DavidG-Develop@users.noreply.github.com> --- nav2_behavior_tree/CMakeLists.txt | 3 + .../toggle_collision_monitor_service.hpp | 63 ++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 6 + .../toggle_collision_monitor_service.cpp | 42 ++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../test_toggle_collision_monitor_service.cpp | 142 ++++++++++++++++++ 6 files changed, 260 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp create mode 100644 nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 2004b5c62b3..f2daaaeeeba 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -255,6 +255,9 @@ list(APPEND plugin_libs nav2_compute_and_track_route_bt_node) add_library(nav2_compute_route_bt_node SHARED plugins/action/compute_route_action.cpp) list(APPEND plugin_libs nav2_compute_route_bt_node) +add_library(nav2_toggle_collision_monitor_service_bt_node SHARED plugins/action/toggle_collision_monitor_service.cpp) +list(APPEND plugin_libs nav2_toggle_collision_monitor_service_bt_node) + foreach(bt_plugin ${plugin_libs}) target_include_directories(${bt_plugin} PRIVATE diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp new file mode 100644 index 00000000000..a06b841d1c3 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2025 David Grbac +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include "nav2_behavior_tree/bt_service_node.hpp" +#include "nav2_msgs/srv/toggle.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Toggle + * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing. + * It will re-initialize when halted. + */ +class ToggleCollisionMonitorService : public BtServiceNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ToggleCollisionMonitorService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ + ToggleCollisionMonitorService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf); + + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ + void on_tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort( + "enable", true, + "Whether to enable or disable collision monitoring"), + }); + } +}; + +} // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index eabba3a4b09..d555f0df948 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -311,6 +311,12 @@ Stamped extracted pose + + Whether to enable collision monitoring + Service name + Server timeout + + Global reference frame robot base frame diff --git a/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp b/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp new file mode 100644 index 00000000000..02553ac2207 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2025 David Grbac +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp" + +namespace nav2_behavior_tree +{ + +ToggleCollisionMonitorService::ToggleCollisionMonitorService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf) +: BtServiceNode(service_node_name, conf) +{ +} + +void ToggleCollisionMonitorService::on_tick() +{ + getInput("enable", request_->enable); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "ToggleCollisionMonitor"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 742b6487fcf..5bd9e2c69e6 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -90,3 +90,7 @@ plugin_add_test(test_smoother_selector_node test_smoother_selector_node.cpp nav2 plugin_add_test(test_goal_checker_selector_node test_goal_checker_selector_node.cpp nav2_goal_checker_selector_bt_node) plugin_add_test(test_progress_checker_selector_node test_progress_checker_selector_node.cpp nav2_progress_checker_selector_bt_node) + +plugin_add_test(test_toggle_collision_monitor_service + test_toggle_collision_monitor_service.cpp + nav2_toggle_collision_monitor_service_bt_node) diff --git a/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp new file mode 100644 index 00000000000..01ff34c968c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2025 David Grbac +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "behaviortree_cpp/bt_factory.h" + +#include "utils/test_service.hpp" +#include "nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp" + +class ToggleCollisionMonitorService : public TestService +{ +public: + ToggleCollisionMonitorService() + : TestService("toggle_collision_monitor") + {} +}; + +class ToggleCollisionMonitorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("toggle_collision_monitor_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); + config_->blackboard->set("initial_pose_received", false); + + factory_->registerNodeType( + "ToggleCollisionMonitor"); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr server_; + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +nav2::LifecycleNode::SharedPtr ToggleCollisionMonitorTestFixture::node_ = nullptr; +std::shared_ptr +ToggleCollisionMonitorTestFixture::server_ = nullptr; +BT::NodeConfiguration * ToggleCollisionMonitorTestFixture::config_ = nullptr; +std::shared_ptr +ToggleCollisionMonitorTestFixture::factory_ = nullptr; +std::shared_ptr ToggleCollisionMonitorTestFixture::tree_ = nullptr; + +class ToggleParamTest + : public ToggleCollisionMonitorTestFixture, + public ::testing::WithParamInterface {}; + +TEST_P(ToggleParamTest, test_tick) +{ + const bool enable = GetParam(); + + std::string xml_txt = + std::string( + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); +} + +INSTANTIATE_TEST_SUITE_P(EnableDisable, ToggleParamTest, ::testing::Values(true, false)); + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + ToggleCollisionMonitorTestFixture::server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(ToggleCollisionMonitorTestFixture::server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} From 4e5273a2dd399f55db2c54e3d17e2cbb59bc3bcd Mon Sep 17 00:00:00 2001 From: Abhishekh Reddy Date: Fri, 26 Sep 2025 17:41:24 -0400 Subject: [PATCH 07/16] Add a parameter to set initial enabled state for the collision monitor (#5554) * Added a new parameter to set initial enabled state for the collision monitor Signed-off-by: Abhishekh Reddy * Fixed enabled_at_start parameter default value Signed-off-by: Abhishekh Reddy * Updated toggle service unit test for code coverage Signed-off-by: Abhishekh Reddy * Updated enabled state log message on startup Co-authored-by: Steve Macenski Signed-off-by: Abhishekh Reddy * Updated enabled state log message on startup Co-authored-by: Steve Macenski Signed-off-by: Abhishekh Reddy * Renamed parameter to 'enabled' Signed-off-by: Abhishekh Reddy --------- Signed-off-by: Abhishekh Reddy Co-authored-by: Steve Macenski --- nav2_bringup/params/nav2_params.yaml | 1 + .../params/collision_monitor_params.yaml | 1 + .../src/collision_monitor_node.cpp | 9 +++++++++ .../test/collision_monitor_node_test.cpp | 18 ++++++++++++------ 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index e66d409478f..f1d959f4873 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -485,6 +485,7 @@ velocity_smoother: collision_monitor: ros__parameters: + enabled: True base_frame_id: "base_footprint" odom_frame_id: "odom" cmd_vel_in_topic: "cmd_vel_smoothed" diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 28224362ea9..ad7a5378068 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -1,5 +1,6 @@ collision_monitor: ros__parameters: + enabled: True base_frame_id: "base_footprint" odom_frame_id: "odom" cmd_vel_in_topic: "cmd_vel_smoothed" diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index cf69674e66e..77e2c75cca5 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -102,6 +102,15 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) } } + nav2::declare_parameter_if_not_declared(node, "enabled", rclcpp::ParameterValue(true)); + node->get_parameter("enabled", enabled_); + + if (!enabled_) { + RCLCPP_WARN(get_logger(), "Collision monitor is disabled at startup."); + } else { + RCLCPP_INFO(get_logger(), "Collision monitor is enabled at startup."); + } + return nav2::CallbackReturn::SUCCESS; } diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 625d4cde007..685ea6bbe15 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -302,6 +302,8 @@ Tester::~Tester() void Tester::setCommonParameters() { + cm_->declare_parameter("enabled", rclcpp::ParameterValue(true)); + cm_->declare_parameter( "cmd_vel_in_topic", rclcpp::ParameterValue(CMD_VEL_IN_TOPIC)); cm_->set_parameter( @@ -841,26 +843,30 @@ TEST_F(Tester, testToggleService) addSource(SCAN_NAME, SCAN); setVectors({"Stop"}, {SCAN_NAME}); + // Test the parameter in disabled state + cm_->set_parameter(rclcpp::Parameter("enabled", false)); + // Start collision monitor node cm_->start(); + ASSERT_FALSE(cm_->isEnabled()); auto request = std::make_shared(); - // Disable test - request->enable = false; + // Enable test + request->enable = true; { auto result_future = toggle_client_->async_call(request); ASSERT_TRUE(waitToggle(result_future, 2s)); } - ASSERT_FALSE(cm_->isEnabled()); + ASSERT_TRUE(cm_->isEnabled()); - // Enable test - request->enable = true; + // Disable test + request->enable = false; { auto result_future = toggle_client_->async_call(request); ASSERT_TRUE(waitToggle(result_future, 2s)); } - ASSERT_TRUE(cm_->isEnabled()); + ASSERT_FALSE(cm_->isEnabled()); // Stop the collision monitor cm_->stop(); From 645d09a04c610150e399b64cdd36435758ce8a2a Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 30 Sep 2025 03:17:36 +0800 Subject: [PATCH 08/16] Add isPoseOccupied BT node (#5556) * Add isPoseOccupied BT node Signed-off-by: mini-1235 * Remove cout Signed-off-by: mini-1235 --------- Signed-off-by: mini-1235 --- nav2_behavior_tree/CMakeLists.txt | 3 + .../condition/is_pose_occupied_condition.hpp | 101 +++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 9 + .../condition/is_pose_occupied_condition.cpp | 83 +++++++ .../test/plugins/condition/CMakeLists.txt | 2 + .../condition/test_is_pose_occupied.cpp | 203 ++++++++++++++++++ 6 files changed, 401 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_pose_occupied.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index f2daaaeeeba..851a660ab66 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -120,6 +120,9 @@ list(APPEND plugin_libs nav2_goal_updated_condition_bt_node) add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp) list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node) +add_library(nav2_is_pose_occupied_condition_bt_node SHARED plugins/condition/is_pose_occupied_condition.cpp) +list(APPEND plugin_libs nav2_is_pose_occupied_condition_bt_node) + add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp) list(APPEND plugin_libs nav2_time_expired_condition_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp new file mode 100644 index 00000000000..7ad6bb014a5 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_ + +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/json_export.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/srv/get_costs.hpp" +#include "nav2_ros_common/service_client.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" + + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS when the IsPoseOccupied + * service returns true and FAILURE otherwise + */ +class IsPoseOccupiedCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsPoseOccupiedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsPoseOccupiedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsPoseOccupiedCondition() = delete; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); + + return { + BT::InputPort("pose", "Pose to check if occupied"), + BT::InputPort("service_name", "global_costmap/get_cost_global_costmap"), + BT::InputPort( + "cost_threshold", 254.0, + "Cost threshold for considering a pose occupied"), + BT::InputPort("use_footprint", true, "Whether to use footprint cost"), + BT::InputPort( + "consider_unknown_as_obstacle", false, + "Whether to consider unknown cost as obstacle"), + BT::InputPort("server_timeout"), + }; + } + +private: + nav2::LifecycleNode::SharedPtr node_; + nav2::ServiceClient::SharedPtr client_; + // The timeout value while waiting for a response from the + // get cost service + std::chrono::milliseconds server_timeout_; + bool use_footprint_; + bool consider_unknown_as_obstacle_; + double cost_threshold_; + std::string service_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index d555f0df948..7161f6419ce 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -394,6 +394,15 @@ If unknown cost is considered valid + + Pose to check whether it is occupied + Service name to call GetCosts + The cost threshold above which a pose is considered occupied + Whether to use the footprint cost or the point cost + If unknown cost is considered valid + Server timeout + + Error code diff --git a/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp b/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp new file mode 100644 index 00000000000..8d253e5e0a1 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp" +#include +#include +#include + +namespace nav2_behavior_tree +{ + +IsPoseOccupiedCondition::IsPoseOccupiedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + use_footprint_(true), consider_unknown_as_obstacle_(false), cost_threshold_(254), + service_name_("global_costmap/get_cost_global_costmap") +{ + node_ = config().blackboard->get("node"); + server_timeout_ = config().blackboard->template get("server_timeout"); +} + +void IsPoseOccupiedCondition::initialize() +{ + getInput("service_name", service_name_); + getInput("cost_threshold", cost_threshold_); + getInput("use_footprint", use_footprint_); + getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); + getInput("server_timeout", server_timeout_); + client_ = + node_->create_client( + service_name_, + false /* Does not create and spin an internal executor*/); +} + +BT::NodeStatus IsPoseOccupiedCondition::tick() +{ + if (!BT::isStatusActive(status())) { + initialize(); + } + geometry_msgs::msg::PoseStamped pose; + getInput("pose", pose); + + auto request = std::make_shared(); + request->use_footprint = use_footprint_; + request->poses.push_back(pose); + + auto response = client_->invoke(request, server_timeout_); + + if(!response->success) { + RCLCPP_ERROR( + node_->get_logger(), + "GetCosts service call failed"); + return BT::NodeStatus::FAILURE; + } + + if((response->costs[0] == 255 && !consider_unknown_as_obstacle_) || + response->costs[0] < cost_threshold_) + { + return BT::NodeStatus::FAILURE; + } else { + return BT::NodeStatus::SUCCESS; + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsPoseOccupied"); +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index 51f7a908801..f916d1e7578 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -26,6 +26,8 @@ plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_ba plugin_add_test(test_condition_is_path_valid test_is_path_valid.cpp nav2_is_path_valid_condition_bt_node) +plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_is_pose_occupied_condition_bt_node) + plugin_add_test(test_are_error_codes_present test_are_error_codes_present.cpp nav2_would_a_controller_recovery_help_condition_bt_node) plugin_add_test(test_would_a_controller_recovery_help diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_pose_occupied.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_pose_occupied.cpp new file mode 100644 index 00000000000..bb10d5dc4e4 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_pose_occupied.cpp @@ -0,0 +1,203 @@ +// Copyright (c) 2025 Maurice Alexander Purnawan +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "behaviortree_cpp/bt_factory.h" + +#include "utils/test_service.hpp" +#include "nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + + +class IsPoseOccupiedSuccessService : public TestService +{ +public: + IsPoseOccupiedSuccessService() + : TestService("/global_costmap/get_cost_global_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {254}; + response->success = true; + } +}; + +class IsPoseOccupiedFailureService : public TestService +{ +public: + IsPoseOccupiedFailureService() + : TestService("/local_costmap/get_cost_local_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {255}; + response->success = false; + } +}; + +class IsPoseOccupiedTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("is_pose_occupied_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "IsPoseOccupied", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + success_server_.reset(); + failure_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + static std::shared_ptr success_server_; + static std::shared_ptr failure_server_; + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +nav2::LifecycleNode::SharedPtr IsPoseOccupiedTestFixture::node_ = nullptr; + +BT::NodeConfiguration * IsPoseOccupiedTestFixture::config_ = nullptr; +std::shared_ptr +IsPoseOccupiedTestFixture::success_server_ = nullptr; +std::shared_ptr +IsPoseOccupiedTestFixture::failure_server_ = nullptr; +std::shared_ptr IsPoseOccupiedTestFixture::factory_ = nullptr; +std::shared_ptr IsPoseOccupiedTestFixture::tree_ = nullptr; + +TEST_F(IsPoseOccupiedTestFixture, test_tick_is_pose_occupied_success) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 5.0; + pose.pose.position.y = 0.0; + + config_->blackboard->set("pose", pose); + + tree_->rootNode()->executeTick(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); +} + +TEST_F(IsPoseOccupiedTestFixture, test_tick_is_pose_occupied_fail) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 5.0; + pose.pose.position.y = 0.0; + + config_->blackboard->set("pose", pose); + + tree_->rootNode()->executeTick(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + IsPoseOccupiedTestFixture::success_server_ = + std::make_shared(); + std::thread success_server_thread([]() { + rclcpp::spin(IsPoseOccupiedTestFixture::success_server_); + }); + + IsPoseOccupiedTestFixture::failure_server_ = + std::make_shared(); + std::thread failure_server_thread([]() { + rclcpp::spin(IsPoseOccupiedTestFixture::failure_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + success_server_thread.join(); + failure_server_thread.join(); + + return all_successful; +} From c747df5dfe0d6d84d18bc7c586fdded905ef23d8 Mon Sep 17 00:00:00 2001 From: DavidG-Develop <147402604+DavidG-Develop@users.noreply.github.com> Date: Mon, 29 Sep 2025 22:11:36 +0200 Subject: [PATCH 09/16] Add toggle cm to simple commander API (#5559) * Add toggle cm to simple comander API, fix default toggle cm service in test for consistency Signed-off-by: David G * Fix pre commit, ament flake, add readme Signed-off-by: David G * add toggle msg to __init__.py Signed-off-by: David G * add error handling Signed-off-by: David G * add error handling for clear costmap functions Signed-off-by: David G --------- Signed-off-by: David G --- .../test_toggle_collision_monitor_service.cpp | 4 +- nav2_msgs/srv/__init__.py | 2 + nav2_simple_commander/README.md | 1 + .../nav2_simple_commander/robot_navigator.py | 52 ++++++++++++++++++- 4 files changed, 56 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp index 01ff34c968c..291d74fcdc9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp @@ -26,7 +26,7 @@ class ToggleCollisionMonitorService : public TestService { public: ToggleCollisionMonitorService() - : TestService("toggle_collision_monitor") + : TestService("/collision_monitor/toggle") {} }; @@ -105,7 +105,7 @@ TEST_P(ToggleParamTest, test_tick) R"( - diff --git a/nav2_msgs/srv/__init__.py b/nav2_msgs/srv/__init__.py index 444c4be926b..1316822eb16 100644 --- a/nav2_msgs/srv/__init__.py +++ b/nav2_msgs/srv/__init__.py @@ -10,6 +10,7 @@ from nav2_msgs.srv._reload_dock_database import ReloadDockDatabase from nav2_msgs.srv._save_map import SaveMap from nav2_msgs.srv._set_initial_pose import SetInitialPose +from nav2_msgs.srv._toggle import Toggle __all__ = [ 'ClearCostmapAroundRobot', @@ -24,4 +25,5 @@ 'ReloadDockDatabase', 'SaveMap', 'SetInitialPose', + 'Toggle', ] diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index e84c9f4e57c..52080849820 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -40,6 +40,7 @@ New as of September 2023: the simple navigator constructor will accept a `namesp | clearGlobalCostmap() | Clears the global costmap. | | getGlobalCostmap() | Returns the global costmap, `nav2_msgs/Costmap` | | getLocalCostmap() | Returns the local costmap, `nav2_msgs/Costmap` | +| toggleCollisionMonitor(enable) | Toggles the collision monitor on (`True`) or off (`False`). | | waitUntilNav2Active(navigator='bt_navigator, localizer='amcl') | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. Custom navigator and localizer nodes can be specified | | lifecycleStartup() | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. | | lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. | diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index ea93fc61711..b1a8d91d268 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -31,7 +31,7 @@ from nav2_msgs.msg import Route from nav2_msgs.srv import (ClearCostmapAroundPose, ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap, GetCostmap, LoadMap, - ManageLifecycleNodes) + ManageLifecycleNodes, Toggle) from nav_msgs.msg import Goals, OccupancyGrid, Path import rclpy from rclpy.action import ActionClient @@ -251,6 +251,11 @@ def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> N self.create_client( GetCostmap, 'local_costmap/get_costmap' ) + self.toggle_collision_monitor_srv: Client[ + Toggle.Request, Toggle.Response] = \ + self.create_client( + Toggle, 'collision_monitor/toggle' + ) def destroyNode(self) -> None: self.destroy_node() @@ -1056,6 +1061,11 @@ def clearLocalCostmap(self) -> None: req = ClearEntireCostmap.Request() future = self.clear_costmap_local_srv.call_async(req) rclpy.spin_until_future_complete(self, future) + + result = future.result() + if result is None: + self.error('Clear local costmap request failed!') + return def clearGlobalCostmap(self) -> None: @@ -1065,6 +1075,11 @@ def clearGlobalCostmap(self) -> None: req = ClearEntireCostmap.Request() future = self.clear_costmap_global_srv.call_async(req) rclpy.spin_until_future_complete(self, future) + + result = future.result() + if result is None: + self.error('Clear global costmap request failed!') + return def clearCostmapExceptRegion(self, reset_distance: float) -> None: @@ -1075,6 +1090,11 @@ def clearCostmapExceptRegion(self, reset_distance: float) -> None: req.reset_distance = reset_distance future = self.clear_costmap_except_region_srv.call_async(req) rclpy.spin_until_future_complete(self, future) + + result = future.result() + if result is None: + self.error('Clear costmap except region request failed!') + return def clearCostmapAroundRobot(self, reset_distance: float) -> None: @@ -1085,6 +1105,11 @@ def clearCostmapAroundRobot(self, reset_distance: float) -> None: req.reset_distance = reset_distance future = self.clear_costmap_around_robot_srv.call_async(req) rclpy.spin_until_future_complete(self, future) + + result = future.result() + if result is None: + self.error('Clear costmap around robot request failed!') + return def clearLocalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) -> None: @@ -1096,6 +1121,11 @@ def clearLocalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) req.reset_distance = reset_distance future = self.clear_local_costmap_around_pose_srv.call_async(req) rclpy.spin_until_future_complete(self, future) + + result = future.result() + if result is None: + self.error('Clear local costmap around pose request failed!') + return def clearGlobalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) -> None: @@ -1107,6 +1137,11 @@ def clearGlobalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) req.reset_distance = reset_distance future = self.clear_global_costmap_around_pose_srv.call_async(req) rclpy.spin_until_future_complete(self, future) + + result = future.result() + if result is None: + self.error('Clear global costmap around pose request failed!') + return def getGlobalCostmap(self) -> OccupancyGrid: @@ -1140,6 +1175,21 @@ def getLocalCostmap(self) -> OccupancyGrid: return result.map + def toggleCollisionMonitor(self, enable: bool) -> None: + """Toggle the collision monitor.""" + while not self.toggle_collision_monitor_srv.wait_for_service(timeout_sec=1.0): + self.info('Toggle collision monitor service not available, waiting...') + req = Toggle.Request() + req.enable = enable + future = self.toggle_collision_monitor_srv.call_async(req) + + rclpy.spin_until_future_complete(self, future) + result = future.result() + if result is None: + self.error('Toggle collision monitor request failed!') + + return + def lifecycleStartup(self) -> None: """Startup nav2 lifecycle system.""" self.info('Starting up lifecycle nodes based on lifecycle_manager.') From 2e7105f841cd2985d1dabefe81165b09dab27cc8 Mon Sep 17 00:00:00 2001 From: faseelmo <65114418+faseelmo@users.noreply.github.com> Date: Tue, 30 Sep 2025 17:56:25 +0200 Subject: [PATCH 10/16] doc update (#5564) --- nav2_velocity_smoother/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index cd5c0548653..e760d9eda78 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -25,7 +25,7 @@ This package was created to do the following: This is a lifecycle-component node, using the lifecycle manager for state management and composition for process management. It is designed to take in a command from Nav2's controller server and smooth it for use on robot hardware controllers. -Thusly, it takes in a command via the `cmd_vel` topic and produces a smoothed output on `smoothed_cmd_vel`. +Thusly, it takes in a command via the `cmd_vel` topic and produces a smoothed output on `cmd_vel_smoothed`. The node is designed on a regular timer running at a configurable rate. This is in contrast to simply computing a smoothed velocity command in the callback of each `cmd_vel` input from Nav2. @@ -66,7 +66,7 @@ velocity_smoother: | Topic | Type | Use | |------------------|-------------------------|-------------------------------| -| smoothed_cmd_vel | geometry_msgs/Twist or geometry_msgs/TwistStamped | Publish smoothed velocities | +| cmd_vel_smoothed | geometry_msgs/Twist or geometry_msgs/TwistStamped | Publish smoothed velocities | | cmd_vel | geometry_msgs/Twist or geometry_msgs/TwistStamped | Subscribe to input velocities | From a25b67e56ba931c97bcc0e5d21df4ab41d9c039f Mon Sep 17 00:00:00 2001 From: Saitama Date: Tue, 30 Sep 2025 18:15:37 +0200 Subject: [PATCH 11/16] Fix robot not stopping after reaching dock position (#5568) Publish a zero velocity command after reaching the dock position to ensure that the robot is stopped while waiting for the charge to start. Signed-off-by: agennart Co-authored-by: agennart --- nav2_docking/opennav_docking/src/docking_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index ec7a7fc5e81..d694f90ea87 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -321,6 +321,7 @@ void DockingServer::dockRobot() // We are docked, wait for charging to begin RCLCPP_INFO( get_logger(), "Made contact with dock, waiting for charge to start (if applicable)."); + publishZeroVelocity(); if (waitForCharge(dock)) { if (dock->plugin->isCharger()) { RCLCPP_INFO(get_logger(), "Robot is charging!"); From 9ed832eeeba48dd8d2a63ab4c60ba65aebb2ec6d Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Tue, 30 Sep 2025 23:01:55 +0100 Subject: [PATCH 12/16] Migrate TransformBroadcaster usage to use shared ptr. (#5569) Signed-off-by: Leander Stephen D'Souza --- nav2_route/test/test_goal_intent_extractor.cpp | 4 ++-- nav2_route/test/test_route_tracker.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp index 21c49708193..26077eb25cc 100644 --- a/nav2_route/test/test_goal_intent_extractor.cpp +++ b/nav2_route/test/test_goal_intent_extractor.cpp @@ -83,7 +83,7 @@ TEST(GoalIntentExtractorTest, test_transform_pose) node->get_node_timers_interface()); tf->setCreateTimerInterface(timer_interface); auto transform_listener = std::make_shared(*tf); - tf2_ros::TransformBroadcaster broadcaster(node); + auto broadcaster = std::make_shared(node); std::shared_ptr costmap_subscriber = nullptr; extractor.configure(node, graph, &id_map, tf, costmap_subscriber, "map", "base_link"); @@ -101,7 +101,7 @@ TEST(GoalIntentExtractorTest, test_transform_pose) transform.header.frame_id = "map"; transform.header.stamp = node->now(); transform.child_frame_id = "gps"; - broadcaster.sendTransform(transform); + broadcaster->sendTransform(transform); EXPECT_NO_THROW(extractor.transformPose(pose, "map")); } diff --git a/nav2_route/test/test_route_tracker.cpp b/nav2_route/test/test_route_tracker.cpp index 07ca24a4e91..cfe808b8ac2 100644 --- a/nav2_route/test/test_route_tracker.cpp +++ b/nav2_route/test/test_route_tracker.cpp @@ -69,7 +69,7 @@ TEST(RouteTrackerTest, test_get_robot_pose) node->get_node_timers_interface()); tf->setCreateTimerInterface(timer_interface); auto transform_listener = std::make_shared(*tf); - tf2_ros::TransformBroadcaster broadcaster(node); + auto broadcaster = std::make_shared(node); std::shared_ptr costmap_subscriber; RouteTracker tracker; @@ -81,7 +81,7 @@ TEST(RouteTrackerTest, test_get_robot_pose) transform.header.frame_id = "map"; transform.header.stamp = node->now(); transform.child_frame_id = "base_link"; - broadcaster.sendTransform(transform); + broadcaster->sendTransform(transform); EXPECT_NO_THROW(tracker.getRobotPose()); } From efdfb7dda4c035c18e960eea89adfbf747f1071a Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 2 Oct 2025 01:50:02 +0800 Subject: [PATCH 13/16] Fix CI regression (#5571) Signed-off-by: mini-1235 --- .../src/behaviors/wait/test_wait_behavior_launch.py | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 3c70c5ee797..e3be752fb9f 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -25,13 +25,11 @@ from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml -from nav2_simple_commander.utils import kill_os_processes def generate_launch_description() -> LaunchDescription: sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') nav2_bringup_dir = get_package_share_directory('nav2_bringup') - ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') @@ -72,11 +70,9 @@ def generate_launch_description() -> LaunchDescription: 'GZ_SIM_RESOURCE_PATH', str(Path(os.path.join(sim_dir)).parent.resolve()) ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') - ), - launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], + output='screen', ), IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -133,7 +129,6 @@ def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] ls = LaunchService(argv=argv) ls.include_launch_description(ld) return_code = lts.run(ls) # type: ignore[no-untyped-call] - kill_os_processes('gz sim') return return_code From 185c8b8ea83622eff792c8a9e6e69e9904b418b8 Mon Sep 17 00:00:00 2001 From: Ege Ugur Agus <48795506+eegeugur@users.noreply.github.com> Date: Thu, 2 Oct 2025 00:09:22 +0300 Subject: [PATCH 14/16] Fix optimal plan frame id (#5572) * Fix optimal plan frame id Signed-off-by: Ege Ugur Agus <48795506+eegeugur@users.noreply.github.com> * Remove Whitespaces. Signed-off-by: Ege Ugur Agus <48795506+eegeugur@users.noreply.github.com> * Fix linting error Signed-off-by: Ege Ugur Agus <48795506+eegeugur@users.noreply.github.com> --------- Signed-off-by: Ege Ugur Agus <48795506+eegeugur@users.noreply.github.com> --- nav2_mppi_controller/src/controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index a9fab10c85e..f8c20f4f399 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -116,11 +116,15 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( #endif if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) { + std_msgs::msg::Header trajectory_header; + trajectory_header.stamp = cmd.header.stamp; + trajectory_header.frame_id = costmap_ros_->getGlobalFrameID(); + auto trajectory_msg = utils::toTrajectoryMsg( optimal_trajectory, optimizer_.getOptimalControlSequence(), optimizer_.getSettings().model_dt, - cmd.header); + trajectory_header); opt_traj_pub_->publish(std::move(trajectory_msg)); } From 43fe4d05d77041cc52d6ace9a438389309cd55a3 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 3 Oct 2025 01:32:31 +0800 Subject: [PATCH 15/16] Add Qt6 support for nav2_rviz_plugins (#5576) * Add support for qt6 Signed-off-by: mini-1235 * Modify CMakeList to support Jazzy Signed-off-by: mini-1235 * Precommit Signed-off-by: mini-1235 --------- Signed-off-by: mini-1235 --- nav2_rviz_plugins/CMakeLists.txt | 40 ++++++++++++++++--- .../nav2_rviz_plugins/docking_panel.hpp | 3 ++ .../include/nav2_rviz_plugins/nav2_panel.hpp | 2 + nav2_rviz_plugins/src/nav2_panel.cpp | 16 ++++---- 4 files changed, 48 insertions(+), 13 deletions(-) diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index b917cbffc33..5258fef1f7b 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_route REQUIRED) find_package(pluginlib REQUIRED) -find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rviz_common REQUIRED) @@ -26,6 +25,37 @@ find_package(nav2_ros_common REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) +# In Kilted and older distros, rviz with Qt6 is not supported +if(TARGET Qt5::Core) + set(QT_VERSION_MAJOR 5) + set(QT_VERSION ${Qt5Core_VERSION}) + # In Qt5, state machine is part of core + find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) +elseif(TARGET Qt6::Core) + set(QT_VERSION_MAJOR 6) + set(QT_VERSION ${Qt6Core_VERSION}) + find_package(Qt6 REQUIRED COMPONENTS Core Gui Widgets StateMachine Test Concurrent) +endif() +if(${QT_VERSION} VERSION_LESS 5.15.0) + function(qt_wrap_cpp out) + qt5_wrap_cpp(_sources ${ARGN}) + set("${out}" ${_sources} PARENT_SCOPE) + endfunction() + function(qt_wrap_ui out) + qt5_wrap_ui(_uis_hdrs ${ARGN}) + set("${out}" ${_uis_hdrs} PARENT_SCOPE) + endfunction() +endif() + +set(QT_LIBRARIES + Qt${QT_VERSION_MAJOR}::Core + Qt${QT_VERSION_MAJOR}::Gui + Qt${QT_VERSION_MAJOR}::Widgets +) +if(QT_VERSION_MAJOR EQUAL 6) + list(APPEND QT_LIBRARIES Qt6::StateMachine) +endif() + nav2_package() # We specifically don't turn on CMAKE_AUTOMOC, since it generates one huge @@ -43,10 +73,10 @@ set(nav2_rviz_plugins_headers_to_moc ) foreach(header "${nav2_rviz_plugins_headers_to_moc}") - qt5_wrap_cpp(nav2_rviz_plugins_moc_files "${header}") + qt_wrap_cpp(nav2_rviz_plugins_moc_files "${header}") endforeach() -qt5_wrap_ui(route_tool_UIS_H resource/route_tool.ui) +qt_wrap_ui(route_tool_UIS_H resource/route_tool.ui) # Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html set(CMAKE_INCLUDE_CURRENT_DIR ON) @@ -70,7 +100,6 @@ add_library(${library_name} SHARED ${route_tool_UIS_H} ) target_include_directories(${library_name} PUBLIC - ${Qt5Widgets_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} "$" "$" @@ -90,6 +119,7 @@ target_link_libraries(${library_name} PUBLIC ${std_msgs_TARGETS} tf2_geometry_msgs::tf2_geometry_msgs ${visualization_msgs_TARGETS} + ${QT_LIBRARIES} ) target_link_libraries(${library_name} PRIVATE ament_index_cpp::ament_index_cpp @@ -132,7 +162,7 @@ ament_export_dependencies( nav2_msgs nav2_route nav2_util - Qt5 + Qt${QT_VERSION_MAJOR} rclcpp rclcpp_action rviz_common diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index fcbc1d1b11a..b425a307368 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -18,6 +18,9 @@ // QT #include #include +#include +#include +#include #include #include diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 8c99a270d64..57dd701b1a2 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #undef NO_ERROR #include diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 8da22379965..5a0b3b9acc1 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -870,12 +870,12 @@ Nav2Panel::startThread() void Nav2Panel::onPause() { - QFuture futureNav = + QFuture futureNav = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, client_nav_.get(), std::placeholders::_1), server_timeout_); - QFuture futureLoc = + QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, @@ -885,12 +885,12 @@ Nav2Panel::onPause() void Nav2Panel::onResume() { - QFuture futureNav = + QFuture futureNav = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, client_nav_.get(), std::placeholders::_1), server_timeout_); - QFuture futureLoc = + QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, @@ -900,12 +900,12 @@ Nav2Panel::onResume() void Nav2Panel::onStartup() { - QFuture futureNav = + QFuture futureNav = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, client_nav_.get(), std::placeholders::_1), server_timeout_); - QFuture futureLoc = + QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, @@ -915,12 +915,12 @@ Nav2Panel::onStartup() void Nav2Panel::onShutdown() { - QFuture futureNav = + QFuture futureNav = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, client_nav_.get(), std::placeholders::_1), server_timeout_); - QFuture futureLoc = + QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, From 2a2a709d4ee5015a63b2b9453578cf1d8b52f1d5 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Thu, 2 Oct 2025 18:44:31 +0100 Subject: [PATCH 16/16] Use the new declare_or_get_parameter API for nav2_behavior_tree (#5552) * Use the new declare_or_get_parameter API for nav2_behavior_tree Signed-off-by: Leander Stephen D'Souza * Simplified the parameter implemenation for error_code_name_prefixes. Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- .../bt_action_server_impl.hpp | 83 +++++++------------ .../condition/goal_reached_condition.cpp | 5 +- 2 files changed, 33 insertions(+), 55 deletions(-) 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 f229ede1fd7..b10e8c32040 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 @@ -61,21 +61,7 @@ BtActionServer::BtActionServer( logger_ = node->get_logger(); clock_ = node->get_clock(); - // Declare this node's parameters - if (!node->has_parameter("bt_loop_duration")) { - node->declare_parameter("bt_loop_duration", 10); - } - 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); - } - - std::vector error_code_name_prefixes = { + std::vector default_error_code_name_prefixes = { "assisted_teleop", "backup", "compute_path", @@ -95,32 +81,26 @@ BtActionServer::BtActionServer( " Please review migration guide and update your configuration."); } - if (!node->has_parameter("error_code_name_prefixes")) { - const rclcpp::ParameterValue value = node->declare_parameter( - "error_code_name_prefixes", - rclcpp::PARAMETER_STRING_ARRAY); - if (value.get_type() == rclcpp::PARAMETER_NOT_SET) { - std::string error_code_name_prefixes_str; - for (const auto & error_code_name_prefix : error_code_name_prefixes) { - error_code_name_prefixes_str += " " + error_code_name_prefix; - } - RCLCPP_WARN_STREAM( - logger_, "error_code_name_prefixes parameters were not set. Using default values of:" - << error_code_name_prefixes_str + "\n" - << "Make sure these match your BT and there are not other sources of error codes you" - "reported to your application"); - rclcpp::Parameter error_code_name_prefixes_param("error_code_name_prefixes", - error_code_name_prefixes); - node->set_parameter(error_code_name_prefixes_param); - } else { - error_code_name_prefixes = value.get>(); - std::string error_code_name_prefixes_str; - for (const auto & error_code_name_prefix : error_code_name_prefixes) { - error_code_name_prefixes_str += " " + error_code_name_prefix; - } - RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" - << error_code_name_prefixes_str); - } + // Declare and get error code name prefixes parameter + error_code_name_prefixes_ = node->declare_or_get_parameter( + "error_code_name_prefixes", + default_error_code_name_prefixes); + + // Provide informative logging about error code prefixes + std::string error_code_name_prefixes_str; + for (const auto & error_code_name_prefix : error_code_name_prefixes_) { + error_code_name_prefixes_str += " " + error_code_name_prefix; + } + + if (error_code_name_prefixes_ == default_error_code_name_prefixes) { + RCLCPP_WARN_STREAM( + logger_, "error_code_name_prefixes parameters were not set. Using default values of:" + << error_code_name_prefixes_str + "\n" + << "Make sure these match your BT and there are not other sources of error codes you" + << "reported to your application"); + } else { + RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" + << error_code_name_prefixes_str); } } @@ -170,16 +150,17 @@ bool BtActionServer::on_configure() nullptr, std::chrono::milliseconds(500), false); // Get parameters for BT timeouts - int bt_loop_duration; - node->get_parameter("bt_loop_duration", bt_loop_duration); - bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration); - int default_server_timeout; - node->get_parameter("default_server_timeout", default_server_timeout); - default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); - 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_); + bt_loop_duration_ = std::chrono::milliseconds( + node->declare_or_get_parameter("bt_loop_duration", 10)); + + default_server_timeout_ = std::chrono::milliseconds( + node->declare_or_get_parameter("default_server_timeout", 20)); + + wait_for_service_timeout_ = std::chrono::milliseconds( + node->declare_or_get_parameter("wait_for_service_timeout", 1000)); + + always_reload_bt_ = node->declare_or_get_parameter( + "always_reload_bt_xml", false); // Get error code id names to grab off of the blackboard error_code_name_prefixes_ = node->get_parameter("error_code_name_prefixes").as_string_array(); diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 385dfae6a01..0a4bfd68c7a 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -44,10 +44,7 @@ void GoalReachedCondition::initialize() { node_ = config().blackboard->get("node"); - nav2::declare_parameter_if_not_declared( - node_, "goal_reached_tol", - rclcpp::ParameterValue(0.25)); - node_->get_parameter_or("goal_reached_tol", goal_reached_tol_, 0.25); + goal_reached_tol_ = node_->declare_or_get_parameter("goal_reached_tol", 0.25); tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_);