From bddadb43ab2c83fb407c7553bae87c121b7b8fc1 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 18 Sep 2025 21:04:32 +0300 Subject: [PATCH 1/4] backup Signed-off-by: silanus23 --- .../nav2_behavior_tree/bt_action_node.hpp | 26 ++++- .../nav2_behavior_tree/bt_action_server.hpp | 3 + .../bt_action_server_impl.hpp | 5 +- .../plugins/action/test_bt_action_node.cpp | 96 ++++++++++++++++++- 4 files changed, 127 insertions(+), 3 deletions(-) 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..1073aa0f481 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 (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,8 @@ 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_; + bool is_global_ {false}; + uint64_t last_run_id_ {0}; }; } // 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..8dda53e9ac8 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; } @@ -482,6 +488,94 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // is at least 1000000 x 50 ms 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", 200ms); + 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", 200ms); + 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", 20ms); + 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); +} int main(int argc, char ** argv) { From 49c21774793a9445da92c14e0136874b03130186 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 26 Sep 2025 01:15:08 +0300 Subject: [PATCH 2/4] test fix Signed-off-by: silanus23 --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 1073aa0f481..9bb780372b7 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 @@ -222,7 +222,7 @@ class BtActionNode : public BT::ActionNodeBase } else { if (!BT::isStatusActive(status())) { needs_initialization_ = true; - } + } } if (needs_initialization_) { 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 8dda53e9ac8..793ea556a4f 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 @@ -561,7 +561,7 @@ TEST_F(BTActionNodeTestFixture, test_run_id_non_global_mode_unaffected) R"( - + )"; From b7c91c82dbaa4aa880b807fd5cf3086e9d2cda36 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 26 Sep 2025 02:49:59 +0300 Subject: [PATCH 3/4] making last_run_id_ sentinel value adding more tests. Signed-off-by: silanus23 --- .../nav2_behavior_tree/bt_action_node.hpp | 4 +- .../plugins/action/test_bt_action_node.cpp | 94 ++++++++++++++++++- 2 files changed, 93 insertions(+), 5 deletions(-) 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 9bb780372b7..7b232b2f8b1 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 @@ -521,8 +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 initialization bool is_global_ {false}; - uint64_t last_run_id_ {0}; + uint64_t last_run_id_ {UINT64_MAX}; }; } // namespace nav2_behavior_tree 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 793ea556a4f..534195103b6 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 @@ -488,6 +488,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // is at least 1000000 x 50 ms EXPECT_EQ(ticks, 7); } + TEST_F(BTActionNodeTestFixture, test_run_id_initialization_and_persistence) { // create tree with is_global="true" to enable RunID checking @@ -499,7 +500,7 @@ TEST_F(BTActionNodeTestFixture, test_run_id_initialization_and_persistence) )"; - config_->blackboard->set("server_timeout", 200ms); + config_->blackboard->set("server_timeout", 100ms); config_->blackboard->set("bt_loop_duration", 10ms); // Set initial RunID @@ -531,7 +532,7 @@ TEST_F(BTActionNodeTestFixture, test_run_id_changes_trigger_reinitialization) )"; - config_->blackboard->set("server_timeout", 200ms); + config_->blackboard->set("server_timeout", 100ms); config_->blackboard->set("bt_loop_duration", 10ms); tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); @@ -561,11 +562,11 @@ TEST_F(BTActionNodeTestFixture, test_run_id_non_global_mode_unaffected) R"( - + )"; - config_->blackboard->set("server_timeout", 20ms); + config_->blackboard->set("server_timeout", 100ms); config_->blackboard->set("bt_loop_duration", 10ms); tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); @@ -577,6 +578,91 @@ TEST_F(BTActionNodeTestFixture, test_run_id_non_global_mode_unaffected) 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(10ns); + + auto result = tree_->tickOnce(); + EXPECT_EQ(result, BT::NodeStatus::RUNNING); + + tree_->haltTree(); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From c4acce047a88b89be012c037706bf53680257bb4 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 26 Sep 2025 04:20:13 +0300 Subject: [PATCH 4/4] lint fixing and longer server time for test Signed-off-by: silanus23 --- .../include/nav2_behavior_tree/bt_action_node.hpp | 4 ++-- .../test/plugins/action/test_bt_action_node.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 7b232b2f8b1..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 @@ -220,7 +220,7 @@ class BtActionNode : public BT::ActionNodeBase } } } else { - if (!BT::isStatusActive(status())) { + if (!BT::isStatusActive(status())) { needs_initialization_ = true; } } @@ -522,7 +522,7 @@ 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 initialization +// 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}; }; 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 534195103b6..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 @@ -655,7 +655,7 @@ TEST_F(BTActionNodeTestFixture, test_run_id_zero_edge_case) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); action_server_->setHandleGoalSleepDuration(2ms); - action_server_->setServerLoopRate(10ns); + action_server_->setServerLoopRate(10ms); auto result = tree_->tickOnce(); EXPECT_EQ(result, BT::NodeStatus::RUNNING);