Add PipelineSequence, RecoveryNode, and bt_conversions unit tests (#1821)

* Refactor recovery node

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add recovery node tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add pipeline sequence tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add bt_conversions unit tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp

+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
+
+#include <string>
+#include "behaviortree_cpp_v3/control_node.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+namespace nav2_behavior_tree
+{
+
+/** @brief Type of sequence node that re-ticks previous children when a child returns running
+ *
+ * Type of Control Node  | Child Returns Failure | Child Returns Running
+ * ---------------------------------------------------------------------
+ *  PipelineSequence     |      Restart          | Tick All Previous Again
+ *
+ * Tick All Previous Again means every node up till this one will be reticked. Even
+ * if a previous node returns Running, the next node will be reticked.
+ *
+ * As an example, let's say this node has 3 children: A, B and C. At the start,
+ * they are all IDLE.
+ * |    A    |    B    |    C    |
+ * --------------------------------
+ * |  IDLE   |  IDLE   |  IDLE   |
+ * | RUNNING |  IDLE   |  IDLE   |  - at first A gets ticked. Assume it returns RUNNING
+ *                                  - PipelineSequence returns RUNNING and no other nodes are ticked.
+ * | SUCCESS | RUNNING |  IDLE   |  - This time A returns SUCCESS so B gets ticked as well
+ *                                  - PipelineSequence returns RUNNING and C is not ticked yet
+ * | RUNNING | SUCCESS | RUNNING |  - A gets ticked and returns RUNNING, but since it had previously
+ *                                  - returned SUCCESS, PipelineSequence continues on and ticks B.
+ *                                  - Since B also returns SUCCESS, C gets ticked this time as well.
+ * | RUNNING | SUCCESS | SUCCESS |  - A is still RUNNING, and B returns SUCCESS again. This time C
+ *                                  - returned SUCCESS, ending the sequence. PipelineSequence
+ *                                  - returns SUCCESS and halts A.
+ *
+ * If any children at any time had returned FAILURE. PipelineSequence would have returned FAILURE
+ * and halted all children, ending the sequence.
+ *
+ * Usage in XML: <PipelineSequence>
+ */
+class PipelineSequence : public BT::ControlNode
+{
+public:
+  explicit PipelineSequence(const std::string & name);
+  PipelineSequence(const std::string & name, const BT::NodeConfiguration & config);
+  void halt() override;
+  static BT::PortsList providedPorts() {return {};}
+
+protected:
+  BT::NodeStatus tick() override;
+  std::size_t last_child_ticked_ = 0;
+};
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp

+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
+
+#include <string>
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace nav2_behavior_tree
+{
+/**
+ * @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
+ * returns SUCCESS.
+ *
+ * - If the first child returns FAILURE, the second child will be executed.  After that the first
+ * child is executed again if the second child returns SUCCESS.
+ *
+ * - If the first or second child returns RUNNING, this node returns RUNNING.
+ *
+ * - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE.
+ *
+ */
+class RecoveryNode : public BT::ControlNode
+{
+public:
+  RecoveryNode(
+    const std::string & name,
+    const BT::NodeConfiguration & conf);
+
+  ~RecoveryNode() override = default;
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<int>("number_of_retries", 1, "Number of retries")
+    };
+  }
+
+private:
+  unsigned int current_child_idx_;
+  unsigned int number_of_retries_;
+  unsigned int retry_count_;
+
+  BT::NodeStatus tick() override;
+  void halt() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_

nav2_behavior_tree/plugins/control/pipeline_sequence.cpp

@@ -15,54 +15,11 @@
 #include <stdexcept>
 #include <sstream>
 #include <string>
-#include "behaviortree_cpp_v3/control_node.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
 
-namespace nav2_behavior_tree
-{
+#include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
 
-/** @brief Type of sequence node that re-ticks previous children when a child returns running
- *
- * Type of Control Node  | Child Returns Failure | Child Returns Running
- * ---------------------------------------------------------------------
- *  PipelineSequence     |      Restart          | Tick All Previous Again
- *
- * Tick All Previous Again means every node up till this one will be reticked. Even
- * if a previous node returns Running, the next node will be reticked.
- *
- * As an example, let's say this node has 3 children: A, B and C. At the start,
- * they are all IDLE.
- * |    A    |    B    |    C    |
- * --------------------------------
- * |  IDLE   |  IDLE   |  IDLE   |
- * | RUNNING |  IDLE   |  IDLE   |  - at first A gets ticked. Assume it returns RUNNING
- *                                  - PipelineSequence returns RUNNING and no other nodes are ticked.
- * | SUCCESS | RUNNING |  IDLE   |  - This time A returns SUCCESS so B gets ticked as well
- *                                  - PipelineSequence returns RUNNING and C is not ticked yet
- * | RUNNING | SUCCESS | RUNNING |  - A gets ticked and returns RUNNING, but since it had previously
- *                                  - returned SUCCESS, PipelineSequence continues on and ticks B.
- *                                  - Since B also returns SUCCESS, C gets ticked this time as well.
- * | RUNNING | SUCCESS | SUCCESS |  - A is still RUNNING, and B returns SUCCESS again. This time C
- *                                  - returned SUCCESS, ending the sequence. PipelineSequence
- *                                  - returns SUCCESS and halts A.
- *
- * If any children at any time had returned FAILURE. PipelineSequence would have returned FAILURE
- * and halted all children, ending the sequence.
- *
- * Usage in XML: <PipelineSequence>
- */
-class PipelineSequence : public BT::ControlNode
+namespace nav2_behavior_tree
 {
-public:
-  explicit PipelineSequence(const std::string & name);
-  PipelineSequence(const std::string & name, const BT::NodeConfiguration & config);
-  void halt() override;
-  static BT::PortsList providedPorts() {return {};}
-
-protected:
-  BT::NodeStatus tick() override;
-  std::size_t last_child_ticked_ = 0;
-};
 
 PipelineSequence::PipelineSequence(const std::string & name)
 : BT::ControlNode(name, {})
@@ -85,7 +42,6 @@ BT::NodeStatus PipelineSequence::tick()
         ControlNode::haltChildren();
         last_child_ticked_ = 0;  // reset
         return status;
-        break;
       case BT::NodeStatus::SUCCESS:
         // do nothing and continue on to the next child. If it is the last child
         // we'll exit the loop and hit the wrap-up code at the end of the method.
@@ -102,7 +58,6 @@ BT::NodeStatus PipelineSequence::tick()
         error_msg << "Invalid node status. Received status " << status <<
           "from child " << children_nodes_[i]->name();
         throw std::runtime_error(error_msg.str());
-        break;
     }
   }
   // Wrap up.

nav2_behavior_tree/plugins/control/recovery_node.cpp

@@ -12,142 +12,112 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_
-#define NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_
-
 #include <string>
-#include "behaviortree_cpp_v3/control_node.h"
+#include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
 
 namespace nav2_behavior_tree
 {
-/**
- * @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
- * returns SUCCESS.
- *
- * - If the first child returns FAILURE, the second child will be executed.  After that the first
- * child is executed again if the second child returns SUCCESS.
- *
- * - If the first or second child returns RUNNING, this node returns RUNNING.
- *
- * - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE.
- *
- */
-class RecoveryNode : public BT::ControlNode
+
+RecoveryNode::RecoveryNode(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ControlNode::ControlNode(name, conf),
+  current_child_idx_(0),
+  number_of_retries_(1),
+  retry_count_(0)
 {
-public:
-  RecoveryNode(
-    const std::string & name,
-    const BT::NodeConfiguration & conf)
-  : BT::ControlNode::ControlNode(name, conf), current_child_idx_(0), retry_count_(0)
-  {
-    getInput("number_of_retries", number_of_retries_);
-  }
+  getInput("number_of_retries", number_of_retries_);
+}
 
-  ~RecoveryNode() override = default;
+BT::NodeStatus RecoveryNode::tick()
+{
+  const unsigned children_count = children_nodes_.size();
 
-  // Any BT node that accepts parameters must provide a requiredNodeParameters method
-  static BT::PortsList providedPorts()
-  {
-    return {
-      BT::InputPort<int>("number_of_retries", 1, "Number of retries")
-    };
+  if (children_count != 2) {
+    throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
   }
 
-private:
-  unsigned int current_child_idx_;
-  unsigned int number_of_retries_;
-  unsigned int retry_count_;
-  BT::NodeStatus tick() override
-  {
-    const unsigned children_count = children_nodes_.size();
-
-    if (children_count != 2) {
-      throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
-    }
-
-    setStatus(BT::NodeStatus::RUNNING);
-
-    while (current_child_idx_ < children_count && retry_count_ < number_of_retries_) {
-      TreeNode * child_node = children_nodes_[current_child_idx_];
-      const BT::NodeStatus child_status = child_node->executeTick();
-
-      if (current_child_idx_ == 0) {
-        switch (child_status) {
-          case BT::NodeStatus::SUCCESS:
-            {
-              retry_count_ = 0;
-              halt();
-              return BT::NodeStatus::SUCCESS;
-            }
-            break;
-
-          case BT::NodeStatus::FAILURE:
-            {
-              // tick second child
-              if (retry_count_ <= number_of_retries_) {
-                current_child_idx_++;
-                break;
-              } else {
-                ControlNode::haltChildren();
-                return BT::NodeStatus::FAILURE;
-              }
-            }
-            break;
-
-          case BT::NodeStatus::RUNNING:
-            {
-              return BT::NodeStatus::RUNNING;
-            }
-            break;
-
-          default:
-            {
-            }
-        }  // end switch
-
-      } else if (current_child_idx_ == 1) {
-        switch (child_status) {
-          case BT::NodeStatus::SUCCESS:
-            {
-              retry_count_++;
-              current_child_idx_--;
+  setStatus(BT::NodeStatus::RUNNING);
+
+  while (current_child_idx_ < children_count && retry_count_ < number_of_retries_) {
+    TreeNode * child_node = children_nodes_[current_child_idx_];
+    const BT::NodeStatus child_status = child_node->executeTick();
+
+    if (current_child_idx_ == 0) {
+      switch (child_status) {
+        case BT::NodeStatus::SUCCESS:
+          {
+            retry_count_ = 0;
+            halt();
+            return BT::NodeStatus::SUCCESS;
+          }
+
+        case BT::NodeStatus::FAILURE:
+          {
+            // tick second child
+            if (retry_count_ <= number_of_retries_) {
+              current_child_idx_++;
+              break;
+            } else {
               ControlNode::haltChildren();
-            }
-            break;
-
-          case BT::NodeStatus::FAILURE:
-            {
-              current_child_idx_--;
-              retry_count_ = 0;
-              halt();
               return BT::NodeStatus::FAILURE;
             }
-            break;
-
-          case BT::NodeStatus::RUNNING:
-            {
-              return BT::NodeStatus::RUNNING;
-            }
-            break;
-
-          default:
-            {
-            }
-        }  // end switch
-      }
-    }  // end while loop
-    retry_count_ = 0;
-    halt();
-    return BT::NodeStatus::FAILURE;
-  }
+          }
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+
+        default:
+          {
+            halt();
+            return BT::NodeStatus::FAILURE;
+          }
+      }  // end switch
+
+    } else if (current_child_idx_ == 1) {
+      switch (child_status) {
+        case BT::NodeStatus::SUCCESS:
+          {
+            retry_count_++;
+            current_child_idx_--;
+            ControlNode::haltChildren();
+          }
+          break;
+
+        case BT::NodeStatus::FAILURE:
+          {
+            current_child_idx_--;
+            retry_count_ = 0;
+            halt();
+            return BT::NodeStatus::FAILURE;
+          }
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+
+        default:
+          {
+            halt();
+            return BT::NodeStatus::FAILURE;
+          }
+      }  // end switch
+    }
+  }  // end while loop
+  retry_count_ = 0;
+  halt();
+  return BT::NodeStatus::FAILURE;
+}
 
-  void halt() override
-  {
-    ControlNode::halt();
-    current_child_idx_ = 0;
-    retry_count_ = 0;
-  }
-};
+void RecoveryNode::halt()
+{
+  ControlNode::halt();
+  current_child_idx_ = 0;
+  retry_count_ = 0;
+}
 
 }  // namespace nav2_behavior_tree
 
@@ -156,5 +126,3 @@ BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<nav2_behavior_tree::RecoveryNode>("RecoveryNode");
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_

nav2_behavior_tree/test/CMakeLists.txt

+ament_add_gtest(test_bt_conversions test_bt_conversions.cpp)
+ament_target_dependencies(test_bt_conversions ${dependencies})
+
 add_subdirectory(plugins/condition)
 add_subdirectory(plugins/decorator)
+add_subdirectory(plugins/control)

nav2_behavior_tree/test/plugins/control/CMakeLists.txt

+ament_add_gtest(test_control_recovery_node test_recovery_node.cpp)
+target_link_libraries(test_control_recovery_node nav2_recovery_node_bt_node)
+ament_target_dependencies(test_control_recovery_node ${dependencies})
+
+ament_add_gtest(test_control_pipeline_sequence test_pipeline_sequence.cpp)
+target_link_libraries(test_control_pipeline_sequence nav2_pipeline_sequence_bt_node)
+ament_target_dependencies(test_control_pipeline_sequence ${dependencies})

nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
+
+class PipelineSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp() override
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::PipelineSequence>(
+      "pipeline_sequence", *config_);
+    first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->addChild(first_child_.get());
+    bt_node_->addChild(second_child_.get());
+    bt_node_->addChild(third_child_.get());
+  }
+
+  void TearDown() override
+  {
+    first_child_.reset();
+    second_child_.reset();
+    third_child_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::PipelineSequence> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
+};
+
+std::shared_ptr<nav2_behavior_tree::PipelineSequence>
+PipelineSequenceTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+PipelineSequenceTestFixture::first_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+PipelineSequenceTestFixture::second_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+PipelineSequenceTestFixture::third_child_ = nullptr;
+
+TEST_F(PipelineSequenceTestFixture, test_failure_on_idle_child)
+{
+  first_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_THROW(bt_node_->executeTick(), std::runtime_error);
+}
+
+TEST_F(PipelineSequenceTestFixture, test_failure)
+{
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+TEST_F(PipelineSequenceTestFixture, test_behavior)
+{
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::IDLE);
+  third_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::RUNNING);
+  third_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
+
+class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp() override
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::RecoveryNode>(
+      "recovery_node", *config_);
+    first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->addChild(first_child_.get());
+    bt_node_->addChild(second_child_.get());
+  }
+
+  void TearDown() override
+  {
+    first_child_.reset();
+    second_child_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::RecoveryNode> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
+};
+
+std::shared_ptr<nav2_behavior_tree::RecoveryNode> RecoveryNodeTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode> RecoveryNodeTestFixture::first_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode> RecoveryNodeTestFixture::second_child_ = nullptr;
+
+TEST_F(RecoveryNodeTestFixture, test_only_two_children)
+{
+  nav2_behavior_tree::DummyNode dummy;
+  bt_node_->addChild(&dummy);
+  EXPECT_THROW(bt_node_->executeTick(), BT::BehaviorTreeException);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_running)
+{
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child)
+{
+  first_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_success)
+{
+  // first child returns success right away
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  // first child fails, second child succeeds, then first child succeeds
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+
+  // first child fails, second child succeeds, then first child fails,
+  // second child succeeds, and first child succeeds (one retry)
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_failure)
+{
+  // first child fails, second child fails
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+
+  // first child fails, second child succeeds, then first child fails,
+  // second child succeeds, and first child fails again (one retry)
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

nav2_behavior_tree/test/test_bt_conversions.cpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <chrono>
+#include <string>
+
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/bt_conversions.hpp"
+
+template<typename T>
+class TestNode : public BT::SyncActionNode
+{
+public:
+  TestNode(const std::string & name, const BT::NodeConfiguration & config)
+  : SyncActionNode(name, config)
+  {}
+
+  BT::NodeStatus tick() override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<T>("test")
+    };
+  }
+};
+
+
+TEST(PointPortTest, test_wrong_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <PointPort test="1.0;2.0;3.0;4.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>("PointPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  geometry_msgs::msg::Point value;
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.x, 0.0);
+  EXPECT_EQ(value.y, 0.0);
+  EXPECT_EQ(value.z, 0.0);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <PointPort test="1.0;2.0" />
+        </BehaviorTree>
+      </root>)";
+
+  tree = factory.createTreeFromText(xml_txt);
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.x, 0.0);
+  EXPECT_EQ(value.y, 0.0);
+  EXPECT_EQ(value.z, 0.0);
+}
+
+TEST(PointPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <PointPort test="1.0;2.0;3.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>("PointPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  geometry_msgs::msg::Point value;
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.x, 1.0);
+  EXPECT_EQ(value.y, 2.0);
+  EXPECT_EQ(value.z, 3.0);
+}
+
+TEST(QuaternionPortTest, test_wrong_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <QuaternionPort test="1.0;2.0;3.0;4.0;5.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>("QuaternionPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  geometry_msgs::msg::Quaternion value;
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.x, 0.0);
+  EXPECT_EQ(value.y, 0.0);
+  EXPECT_EQ(value.z, 0.0);
+  EXPECT_EQ(value.w, 1.0);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <QuaternionPort test="1.0;2.0;3.0" />
+        </BehaviorTree>
+      </root>)";
+
+  tree = factory.createTreeFromText(xml_txt);
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.x, 0.0);
+  EXPECT_EQ(value.y, 0.0);
+  EXPECT_EQ(value.z, 0.0);
+  EXPECT_EQ(value.w, 1.0);
+}
+
+TEST(QuaternionPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <QuaternionPort test="0.7;0.0;0.0;0.7" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>("QuaternionPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  geometry_msgs::msg::Quaternion value;
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.x, 0.7);
+  EXPECT_EQ(value.y, 0.0);
+  EXPECT_EQ(value.z, 0.0);
+  EXPECT_EQ(value.w, 0.7);
+}
+
+TEST(MillisecondsPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <MillisecondsPort test="10000" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<std::chrono::milliseconds>>("MillisecondsPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  std::chrono::milliseconds value;
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.count(), 10000);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <MillisecondsPort test="123.4" />
+        </BehaviorTree>
+      </root>)";
+
+  tree = factory.createTreeFromText(xml_txt);
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.count(), 123);
+}