Add runid

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<std::string>("server_name", "Action server name"),
-      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+      BT::InputPort<std::chrono::milliseconds>("server_timeout"),
+      BT::InputPort<bool>("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<uint64_t>("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

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

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -196,7 +196,7 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
   blackboard_->set<std::chrono::milliseconds>(
     "wait_for_service_timeout",
     wait_for_service_timeout_);
-
+  blackboard_->set<uint64_t>("run_id", run_id_);  // NOLINT
   return true;
 }
 
@@ -336,6 +336,9 @@ void BtActionServer<ActionT, NodeT>::executeCallback()
     return;
   }
 
+  run_id_++;
+  blackboard_->set<uint64_t>("run_id", run_id_);
+
   auto is_canceling = [&]() {
       if (action_server_ == nullptr) {
         RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling.");

nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

@@ -147,7 +147,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::actio
 
   BT::NodeStatus on_cancelled() override
   {
-    config().blackboard->set("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<int>());
+    }
     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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="100" is_global="true" />
+        </BehaviorTree>
+      </root>)";
+
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  // Set initial RunID
+  config_->blackboard->set<uint64_t>("run_id", 1);
+
+  tree_ = std::make_shared<BT::Tree>(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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="50" is_global="true" />
+        </BehaviorTree>
+      </root>)";
+
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  tree_ = std::make_shared<BT::Tree>(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<uint64_t>("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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="10" />
+        </BehaviorTree>
+      </root>)";
+
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  tree_ = std::make_shared<BT::Tree>(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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="5" is_global="true" />
+        </BehaviorTree>
+      </root>)";
+
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  // Don't set run_id - test graceful handling
+  tree_ = std::make_shared<BT::Tree>(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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="100" is_global="true" />
+        </BehaviorTree>
+      </root>)";
+
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+  config_->blackboard->set<uint64_t>("run_id", 1);
+
+  tree_ = std::make_shared<BT::Tree>(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<uint64_t>("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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="5" is_global="true" />
+        </BehaviorTree>
+      </root>)";
+
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  // Test with RunID = 0 (edge case)
+  config_->blackboard->set<uint64_t>("run_id", 0);
+
+  tree_ = std::make_shared<BT::Tree>(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);