diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 682dc7a9a09..d8bff1d03bc 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -77,6 +77,7 @@ class BtActionNode : public BT::ActionNodeBase if (getInput("server_name", remapped_action_name)) { action_name_ = remapped_action_name; } + getInput("is_global", is_global_); createActionClient(action_name_); // Give the derive class a chance to do any initialization @@ -121,7 +122,8 @@ class BtActionNode : public BT::ActionNodeBase { BT::PortsList basic = { BT::InputPort("server_name", "Action server name"), - BT::InputPort("server_timeout") + BT::InputPort("server_timeout"), + BT::InputPort("is_global", false, "Use RunID for initialization") }; basic.insert(addition.begin(), addition.end()); @@ -202,8 +204,20 @@ class BtActionNode : public BT::ActionNodeBase */ BT::NodeStatus tick() override { + bool needs_initialization_ = false; // first step to be done only at the beginning of the Action - if (!BT::isStatusActive(status())) { + if (is_global_) { + std::string current_run_id = config().blackboard->get("run_id"); + if (current_run_id != last_run_id_) { + needs_initialization_ = true; + last_run_id_ = current_run_id; + } + } else { + if (!BT::isStatusActive(status())) { + needs_initialization_ = true; + } + } + if (needs_initialization_) { // reset the flag to send the goal or not, allowing the user the option to set it in on_tick should_send_goal_ = true; @@ -499,6 +513,10 @@ class BtActionNode : public BT::ActionNodeBase // Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent. bool should_send_goal_; + +// Initialized to UINT64_MAX as a sentinel value to ensure the first tick always triggers + bool is_global_ {false}; + std::string last_run_id_; }; } // namespace nav2_behavior_tree 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 aae15723e0f..c96e90ef5b3 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 @@ -21,6 +21,10 @@ #include #include +#include +#include +#include + #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/behavior_tree_engine.hpp" #include "nav2_behavior_tree/ros_topic_logger.hpp" @@ -304,6 +308,10 @@ class BtActionServer // internal error tracking (IOW not behaviorTree blackboard errors) uint16_t internal_error_code_; std::string internal_error_msg_; + + // To keep track of the execution number + boost::uuids::random_generator uuid_generator_; + std::string run_id_; }; } // namespace nav2_behavior_tree 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 74d3700620e..af0a42dd4e5 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 @@ -25,6 +25,10 @@ #include #include +#include +#include +#include + #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" #include "nav2_ros_common/node_utils.hpp" @@ -179,7 +183,8 @@ bool BtActionServer::on_configure() blackboard_->set( "wait_for_service_timeout", wait_for_service_timeout_); - + run_id_ = boost::uuids::to_string(uuid_generator_()); + blackboard_->set("run_id", run_id_); // NOLINT return true; } @@ -362,6 +367,8 @@ void BtActionServer::executeCallback() return; } + run_id_ = boost::uuids::to_string(uuid_generator_()); + blackboard_->set("run_id", run_id_); auto is_canceling = [&]() { if (action_server_ == nullptr) { RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling."); diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index 96e9a594447..68d45a7d8d8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -147,7 +147,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset("sequence", result_.result->sequence); + // Check if result is available before accessing it + if (result_.result) { + config().blackboard->set("sequence", result_.result->sequence); + } else { + // Set empty sequence if no result available + config().blackboard->set("sequence", std::vector()); + } config().blackboard->set("on_cancelled_triggered", true); return BT::NodeStatus::SUCCESS; } @@ -487,6 +493,180 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) EXPECT_EQ(ticks, 7); } +TEST_F(BTActionNodeTestFixture, test_run_id_initialization_and_persistence) +{ + // create tree with is_global="true" to enable RunID checking + std::string xml_txt = + R"( + + + + + )"; + + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + // Set initial RunID + config_->blackboard->set("run_id", 1); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); // Slower server rate + + // First tick should initialize with run_id = 1 + auto result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); + + // Subsequent ticks with same RunID should continue without re-initialization + result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); + + // Clean up by halting the tree + tree_->haltTree(); +} + +TEST_F(BTActionNodeTestFixture, test_run_id_changes_trigger_reinitialization) +{ + std::string xml_txt = + R"( + + + + + )"; + + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // Test with multiple run_id changes + for (uint64_t run_id = 1; run_id <= 3; ++run_id) { + config_->blackboard->set("run_id", run_id); + + // Halt tree to reset state + tree_->haltTree(); + + // First tick with new run_id should start new execution + auto result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING) << "Failed on run_id: " << run_id; + } + + // Final cleanup + tree_->haltTree(); +} + +TEST_F(BTActionNodeTestFixture, test_run_id_non_global_mode_unaffected) +{ + // Test without is_global flag (should use old behavior) + std::string xml_txt = + R"( + + + + + )"; + + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); + + // Should work normally without RunID checking + auto result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); +} + +TEST_F(BTActionNodeTestFixture, test_run_id_missing_from_blackboard) +{ + // Test behavior when run_id is not set - should work like old behavior + std::string xml_txt = + R"( + + + + + )"; + + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + // Don't set run_id - test graceful handling + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); + + // Should work like old behavior when RunID is missing + auto result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); + + tree_->haltTree(); +} + +TEST_F(BTActionNodeTestFixture, test_run_id_change_during_execution) +{ + // Test RunID change while node is already running (key preemption scenario) + std::string xml_txt = + R"( + + + + + )"; + + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("run_id", 1); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); + + // Start execution with run_id = 1 + auto result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); + + // Change RunID while running (simulates new navigation goal) + config_->blackboard->set("run_id", 2); + + // Next tick should detect change and reinitialize + result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); + + tree_->haltTree(); +} + +TEST_F(BTActionNodeTestFixture, test_run_id_zero_edge_case) +{ + std::string xml_txt = + R"( + + + + + )"; + + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + // Test with RunID = 0 (edge case) + config_->blackboard->set("run_id", 0); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ms); + + auto result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); + + tree_->haltTree(); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv);