Implement tests using xml_txt trees and dummy nodes

Signed-off-by: redvinaa <redvinaa@gmail.com>

Author: redvinaa

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp

@@ -69,6 +69,16 @@ const std::map<state_t, uint16_t> child_indices = {
  *
  *     <!-- ON_RESUME branch (optional) -->
  * </Pause>
+ *
+ * The controller starts in RESUMED state, and ticks it until it returns success.
+ * When the pause service is called, ON_PAUSE is ticked until completion,
+ * then the controller switches to PAUSED state.
+ * When the resume service is called, ON_RESUME is ticked until completion,
+ * then the controller switches back to RESUMED state.
+ *
+ * The controller only returns success when the RESUMED child returns success.
+ * The controller returns failure if any child returns failure.
+ * In any other case, it returns running.
  */
 
 
@@ -99,6 +109,11 @@ class PauseResumeController : public BT::ControlNode
     };
   }
 
+  [[nodiscard]] inline state_t getState() const
+  {
+    return state_;
+  }
+
 private:
   //! @brief Set state to PAUSE_REQUESTED
   void pauseServiceCallback(

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

@@ -92,13 +92,19 @@ BT::NodeStatus PauseResumeController::tick()
   // If child is used, tick it
   const BT::NodeStatus child_status =
     children_nodes_[child_indices.at(state_)]->executeTick();
+
   switch (child_status) {
     case BT::NodeStatus::RUNNING:
       return BT::NodeStatus::RUNNING;
     case BT::NodeStatus::SUCCESS:
     case BT::NodeStatus::SKIPPED:
+      if (state_ == RESUMED) {
+        // Resumed child returned SUCCESS, we're done
+        return BT::NodeStatus::SUCCESS;
+      }
       switchToNextState();
-      return BT::NodeStatus::SUCCESS;
+      // If any branch other than RESUMED returned SUCCESS, keep ticking
+      return BT::NodeStatus::RUNNING;
     case BT::NodeStatus::FAILURE:
       RCLCPP_ERROR(
         logger_, "%s child returned FAILURE", state_names.at(state_).c_str());

nav2_behavior_tree/plugins/control/sequence_with_blackboard_memory.cpp

@@ -61,7 +61,7 @@ BT::NodeStatus SequenceWithBlackboardMemoryNode::tick()
   }  // end while loop
 
   // The entire while loop completed. This means that all the children returned SUCCESS.
-  if (current_child_idx == children_count) {
+  if (current_child_idx >= children_count) {
     resetChildren();
     setOutput("current_child_idx", 0);
   }

nav2_behavior_tree/test/plugins/control/CMakeLists.txt

@@ -6,3 +6,6 @@ plugin_add_test(test_control_round_robin_node test_round_robin_node.cpp nav2_rou
 
 plugin_add_test(test_control_pause_resume_controller
   test_pause_resume_controller.cpp nav2_pause_resume_controller_bt_node)
+
+plugin_add_test(test_sequence_with_blackboard_memory
+  test_sequence_with_blackboard_memory.cpp nav2_sequence_with_blackboard_memory_bt_node)

nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp

@@ -17,13 +17,14 @@
 
 #include "utils/test_behavior_tree_fixture.hpp"
 #include "utils/test_dummy_tree_node.hpp"
+#include "utils/get_node_from_tree.hpp"
 #include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp"
 #include "std_srvs/srv/trigger.hpp"
 
 class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
 {
 public:
-  void SetUp() override
+  static void SetUpTestCase()
   {
     node_ = std::make_shared<rclcpp::Node>("pause_resume_controller_test_fixture");
     executor_ =
@@ -36,49 +37,36 @@ class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTree
     resume_client_ = node_->create_client<std_srvs::srv::Trigger>(
       "resume", rclcpp::ServicesQoS(), cb_group_);
 
-    config_->input_ports["pause_service_name"] = "pause";
-    config_->input_ports["resume_service_name"] = "resume";
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    config_ = new BT::NodeConfiguration();
     config_->blackboard = BT::Blackboard::create();
     config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
 
-    bt_node_ = std::make_shared<nav2_behavior_tree::PauseResumeController>(
-      "pause_resume_controller", *config_);
-    resumed_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
-    paused_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
-    on_pause_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
-    on_resume_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
-    resumed_child_->changeStatus(BT::NodeStatus::SUCCESS);
-    paused_child_->changeStatus(BT::NodeStatus::SUCCESS);
-    on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS);
-    on_resume_child_->changeStatus(BT::NodeStatus::SUCCESS);
-    bt_node_->addChild(resumed_child_.get());
-    bt_node_->addChild(paused_child_.get());
-    bt_node_->addChild(on_pause_child_.get());
-    bt_node_->addChild(on_resume_child_.get());
+    factory_->registerNodeType<nav2_behavior_tree::PauseResumeController>("PauseResumeController");
+
+    // Register dummy node for testing
+    factory_->registerNodeType<nav2_behavior_tree::DummyNode>("DummyNode");
+  }
+
+  static void TearDownTestCase()
+  {
+    if (config_) {
+      delete config_;
+      config_ = nullptr;
+    }
+    tree_.reset();
   }
 
 protected:
-  static std::shared_ptr<nav2_behavior_tree::PauseResumeController> bt_node_;
-  static std::shared_ptr<nav2_behavior_tree::DummyNode> paused_child_;
-  static std::shared_ptr<nav2_behavior_tree::DummyNode> resumed_child_;
-  static std::shared_ptr<nav2_behavior_tree::DummyNode> on_pause_child_;
-  static std::shared_ptr<nav2_behavior_tree::DummyNode> on_resume_child_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
   static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
   static rclcpp::CallbackGroup::SharedPtr cb_group_;
   static rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr pause_client_;
   static rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resume_client_;
 };
 
-std::shared_ptr<nav2_behavior_tree::PauseResumeController>
-PauseResumeControllerTestFixture::bt_node_ = nullptr;
-std::shared_ptr<nav2_behavior_tree::DummyNode>
-PauseResumeControllerTestFixture::paused_child_ = nullptr;
-std::shared_ptr<nav2_behavior_tree::DummyNode>
-PauseResumeControllerTestFixture::resumed_child_ = nullptr;
-std::shared_ptr<nav2_behavior_tree::DummyNode>
-PauseResumeControllerTestFixture::on_pause_child_ = nullptr;
-std::shared_ptr<nav2_behavior_tree::DummyNode>
-PauseResumeControllerTestFixture::on_resume_child_ = nullptr;
 rclcpp::executors::SingleThreadedExecutor::SharedPtr
 PauseResumeControllerTestFixture::executor_ = nullptr;
 rclcpp::CallbackGroup::SharedPtr
@@ -87,51 +75,104 @@ rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
 PauseResumeControllerTestFixture::pause_client_ = nullptr;
 rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
 PauseResumeControllerTestFixture::resume_client_ = nullptr;
+BT::NodeConfiguration * PauseResumeControllerTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> PauseResumeControllerTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> PauseResumeControllerTestFixture::tree_ = nullptr;
 
 TEST_F(PauseResumeControllerTestFixture, test_behavior)
 {
-  resumed_child_->changeStatus(BT::NodeStatus::SUCCESS);
-  paused_child_->changeStatus(BT::NodeStatus::SUCCESS);
-  on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS);
-  on_resume_child_->changeStatus(BT::NodeStatus::SUCCESS);
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
-
-  // Set on_pause to RUNNING, call pause service, expect RUNNING
-  on_pause_child_->changeStatus(BT::NodeStatus::RUNNING);
-  auto res = pause_client_->async_send_request(
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <PauseResumeController
+            pause_service_name="pause"
+            resume_service_name="resume">
+            <DummyNode/>  <!-- RESUMED -->
+            <DummyNode/>  <!-- PAUSED -->
+            <DummyNode/>  <!-- ON_PAUSE -->
+            <DummyNode/>  <!-- ON_RESUME -->
+          </PauseResumeController>
+        </BehaviorTree>
+      </root>)";
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // get pause_resume_controller so we can check state
+  auto pause_bt_node =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::PauseResumeController>(tree_);
+  ASSERT_NE(pause_bt_node, nullptr);
+  using state_t = nav2_behavior_tree::state_t;
+
+  // get dummy nodes so we can change their status
+  auto resumed_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 0);
+  auto paused_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 1);
+  auto on_pause_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 2);
+  auto on_resume_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 3);
+  ASSERT_NE(resumed_child, nullptr);
+  ASSERT_NE(paused_child, nullptr);
+  ASSERT_NE(on_pause_child, nullptr);
+  ASSERT_NE(on_resume_child, nullptr);
+
+  resumed_child->changeStatus(BT::NodeStatus::RUNNING);
+  paused_child->changeStatus(BT::NodeStatus::RUNNING);
+  on_pause_child->changeStatus(BT::NodeStatus::RUNNING);
+  on_resume_child->changeStatus(BT::NodeStatus::RUNNING);
+
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+
+  const auto & check_request_succeeded = [](
+    rclcpp::Client<std_srvs::srv::Trigger>::FutureAndRequestId & future)
+    {
+      executor_->spin_until_future_complete(future, std::chrono::seconds(1));
+      ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready);
+      EXPECT_EQ(future.get()->success, true);
+    };
+
+  // Call pause service, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE
+  auto future = pause_client_->async_send_request(
     std::make_shared<std_srvs::srv::Trigger::Request>());
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
-  executor_->spin_until_future_complete(res, std::chrono::seconds(1));
-  ASSERT_EQ(res.wait_for(std::chrono::seconds(1)), std::future_status::ready);
-  EXPECT_EQ(res.get()->success, true);
-
-  // Change on_pause to SUCCESS, paused child to FAILURE
-  // Expect SUCCESS (from on_pause), then FAILURE (from paused child)
-  on_pause_child_->changeStatus(BT::NodeStatus::SUCCESS);
-  paused_child_->changeStatus(BT::NodeStatus::FAILURE);
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
-
-  // Set paused to SUCCESS, tick again, expect SUCCESS
-  paused_child_->changeStatus(BT::NodeStatus::SUCCESS);
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
-
-  // Set on_resume to SKIPPED, call resume service, expect SUCCESS (treated same as SKIPPED)
-  on_resume_child_->changeStatus(BT::NodeStatus::SKIPPED);
-  res = resume_client_->async_send_request(
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE);
+  check_request_succeeded(future);
+
+  // Change ON_PAUSE child to SUCCESS, expect RUNNING and PAUSED
+  on_pause_child->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+
+  // Set PAUSED child to SUCCESS, expect RUNNING and PAUSED
+  // (should keep ticking unless RESUMED branch succeeds)
+  paused_child->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+
+  // Set PAUSED child to SKIPPED, expect RUNNING and PAUSED
+  paused_child->changeStatus(BT::NodeStatus::SKIPPED);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+
+  // Call resume service, change ON_RESUME child to FAILURE, expect FAILURE
+  future = resume_client_->async_send_request(
     std::make_shared<std_srvs::srv::Trigger::Request>());
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
-  executor_->spin_until_future_complete(res, std::chrono::seconds(1));
-  ASSERT_EQ(res.wait_for(std::chrono::seconds(1)), std::future_status::ready);
-  EXPECT_EQ(res.get()->success, true);
-
-  // Set resumed to RUNNING, expect RUNNING
-  resumed_child_->changeStatus(BT::NodeStatus::RUNNING);
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
-
-  // Change resumed to SUCCESS, expect SUCCESS
-  resumed_child_->changeStatus(BT::NodeStatus::SUCCESS);
-  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  on_resume_child->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE);
+  check_request_succeeded(future);
+
+  // Halt the tree, expect RUNNING and RESUMED
+  tree_->haltTree();
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+
+  // Set resumed child to SUCCESS, expect SUCCESS and RESUMED
+  resumed_child->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
 }
 
 int main(int argc, char ** argv)