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,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<std::string>("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

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -21,6 +21,10 @@
 #include <string>
 #include <vector>
 
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
 #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

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -25,6 +25,10 @@
 #include <string>
 #include <vector>
 
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
 #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<ActionT, NodeT>::on_configure()
   blackboard_->set<std::chrono::milliseconds>(
     "wait_for_service_timeout",
     wait_for_service_timeout_);
-
+  run_id_ = boost::uuids::to_string(uuid_generator_());
+  blackboard_->set<std::string>("run_id", run_id_);  // NOLINT
   return true;
 }
 
@@ -362,6 +367,8 @@ void BtActionServer<ActionT, NodeT>::executeCallback()
     return;
   }
 
+  run_id_ = boost::uuids::to_string(uuid_generator_());
+  blackboard_->set<std::string>("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;
   }
@@ -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"(
+      <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);