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..880086221f1 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,28 @@ 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_) { + try { + uint64_t current_run_id = config().blackboard->get("run_id"); + if (current_run_id != last_run_id_) { + needs_initialization_ = true; + last_run_id_ = current_run_id; + } + } catch (const std::exception & e) { + // run_id not found on blackboard, use old behavior + if (!BT::isStatusActive(status())) { + needs_initialization_ = true; + } + } + } 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 +521,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}; + uint64_t last_run_id_ {UINT64_MAX}; }; } // 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 924d1ebc28a..9e0c1aebc1d 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 @@ -298,6 +298,9 @@ 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 + uint64_t run_id_ = 0; }; } // 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 10a8289d5f4..b70349dd116 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 @@ -196,7 +196,7 @@ bool BtActionServer::on_configure() blackboard_->set( "wait_for_service_timeout", wait_for_service_timeout_); - + blackboard_->set("run_id", run_id_); // NOLINT return true; } @@ -336,6 +336,9 @@ void BtActionServer::executeCallback() return; } + run_id_++; + 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 3b2a3b382c1..9fb923e5b4a 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; } @@ -483,6 +489,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);