diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc69d53d33c68a720b5643f4c3e555024475c5f7
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
@@ -0,0 +1,71 @@
+// 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__CONDITION__GOAL_REACHED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp_v3/condition_node.h"
+#include "tf2_ros/buffer.h"
+
+namespace nav2_behavior_tree
+{
+
+class GoalReachedCondition : public BT::ConditionNode
+{
+public:
+  GoalReachedCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  GoalReachedCondition() = delete;
+
+  ~GoalReachedCondition() override;
+
+  BT::NodeStatus tick() override;
+
+  void initialize();
+
+  bool isGoalReached();
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
+      BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
+      BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
+    };
+  }
+
+protected:
+  void cleanup()
+  {}
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  bool initialized_;
+  double goal_reached_tol_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
+  double transform_tolerance_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..45a6704ef908f459701c23820ea85183baae2d4b
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
@@ -0,0 +1,48 @@
+// Copyright (c) 2020 Aitor Miguel Blanco
+//
+// 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__CONDITION__GOAL_UPDATED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
+
+#include <string>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class GoalUpdatedCondition : public BT::ConditionNode
+{
+public:
+  GoalUpdatedCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  GoalUpdatedCondition() = delete;
+
+  BT::NodeStatus tick() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+
+private:
+  geometry_msgs::msg::PoseStamped goal_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9bed7e6ac4d158e4d27132e847ca8819bfd0650d
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp
@@ -0,0 +1,25 @@
+// 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__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+namespace nav2_behavior_tree
+{
+BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node);
+}
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
index db49ab82fafbe3a20ff7d9e412c9451ca04cc045..3a5b8d821c9fdb2027c8815cedddb756f63db5e4 100644
--- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
@@ -12,115 +12,80 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__GOAL_REACHED_CONDITION_HPP_
-#define NAV2_BEHAVIOR_TREE__GOAL_REACHED_CONDITION_HPP_
-
 #include <string>
 #include <memory>
 
-#include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
 #include "nav2_util/robot_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/buffer.h"
 #include "nav2_util/node_utils.hpp"
 
+#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
+
 namespace nav2_behavior_tree
 {
 
-class GoalReachedCondition : public BT::ConditionNode
+GoalReachedCondition::GoalReachedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  initialized_(false),
+  global_frame_("map"),
+  robot_base_frame_("base_link")
 {
-public:
-  GoalReachedCondition(
-    const std::string & condition_name,
-    const BT::NodeConfiguration & conf)
-  : BT::ConditionNode(condition_name, conf),
-    initialized_(false),
-    global_frame_("map"),
-    robot_base_frame_("base_link")
-  {
-    getInput("global_frame", global_frame_);
-    getInput("robot_base_frame", robot_base_frame_);
-  }
-
-  GoalReachedCondition() = delete;
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
+}
 
-  ~GoalReachedCondition()
-  {
-    cleanup();
-  }
+GoalReachedCondition::~GoalReachedCondition()
+{
+  cleanup();
+}
 
-  BT::NodeStatus tick() override
-  {
-    if (!initialized_) {
-      initialize();
-    }
-
-    if (isGoalReached()) {
-      return BT::NodeStatus::SUCCESS;
-    }
-    return BT::NodeStatus::FAILURE;
+BT::NodeStatus GoalReachedCondition::tick()
+{
+  if (!initialized_) {
+    initialize();
   }
 
-  void initialize()
-  {
-    node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-
-    nav2_util::declare_parameter_if_not_declared(
-      node_, "goal_reached_tol",
-      rclcpp::ParameterValue(0.25));
-    node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
-    tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
-
-    node_->get_parameter("transform_tolerance", transform_tolerance_);
-
-    initialized_ = true;
+  if (isGoalReached()) {
+    return BT::NodeStatus::SUCCESS;
   }
+  return BT::NodeStatus::FAILURE;
+}
 
-  bool
-  isGoalReached()
-  {
-    geometry_msgs::msg::PoseStamped current_pose;
+void GoalReachedCondition::initialize()
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
-    if (!nav2_util::getCurrentPose(
-        current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))
-    {
-      RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
-      return false;
-    }
+  nav2_util::declare_parameter_if_not_declared(
+    node_, "goal_reached_tol",
+    rclcpp::ParameterValue(0.25));
+  node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
-    geometry_msgs::msg::PoseStamped goal;
-    getInput("goal", goal);
-    double dx = goal.pose.position.x - current_pose.pose.position.x;
-    double dy = goal.pose.position.y - current_pose.pose.position.y;
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
 
-    return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
-  }
+  initialized_ = true;
+}
 
-  static BT::PortsList providedPorts()
-  {
-    return {
-      BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
-      BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
-      BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
-    };
-  }
+bool GoalReachedCondition::isGoalReached()
+{
+  geometry_msgs::msg::PoseStamped current_pose;
 
-protected:
-  void cleanup()
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))
   {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return false;
   }
 
-private:
-  rclcpp::Node::SharedPtr node_;
-  std::shared_ptr<tf2_ros::Buffer> tf_;
+  geometry_msgs::msg::PoseStamped goal;
+  getInput("goal", goal);
+  double dx = goal.pose.position.x - current_pose.pose.position.x;
+  double dy = goal.pose.position.y - current_pose.pose.position.y;
 
-  bool initialized_;
-  double goal_reached_tol_;
-  std::string global_frame_;
-  std::string robot_base_frame_;
-  double transform_tolerance_;
-};
+  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
+}
 
 }  // namespace nav2_behavior_tree
 
@@ -129,5 +94,3 @@ BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__GOAL_REACHED_CONDITION_HPP_
diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp
index 31f992fe45d6ac970d4ab4c4209a3d9a5abc11f7..a09b9f81df979e45d38783630e3246fa9ae5c498 100644
--- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp
@@ -12,53 +12,35 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_
-#define NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_
-
 #include <string>
 
-#include "behaviortree_cpp_v3/condition_node.h"
-#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
 
 namespace nav2_behavior_tree
 {
 
-class GoalUpdatedCondition : public BT::ConditionNode
+GoalUpdatedCondition::GoalUpdatedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
 {
-public:
-  GoalUpdatedCondition(
-    const std::string & condition_name,
-    const BT::NodeConfiguration & conf)
-  : BT::ConditionNode(condition_name, conf)
-  {
-  }
-
-  GoalUpdatedCondition() = delete;
-
-  BT::NodeStatus tick() override
-  {
-    if (status() == BT::NodeStatus::IDLE) {
-      goal_ = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
-      return BT::NodeStatus::FAILURE;
-    }
-
-    auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
-    if (goal_ != current_goal) {
-      goal_ = current_goal;
-      return BT::NodeStatus::SUCCESS;
-    }
+}
 
+BT::NodeStatus GoalUpdatedCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    goal_ = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
     return BT::NodeStatus::FAILURE;
   }
 
-  static BT::PortsList providedPorts()
-  {
-    return {};
+  auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
+  if (goal_ != current_goal) {
+    goal_ = current_goal;
+    return BT::NodeStatus::SUCCESS;
   }
 
-private:
-  geometry_msgs::msg::PoseStamped goal_;
-};
+  return BT::NodeStatus::FAILURE;
+}
 
 }  // namespace nav2_behavior_tree
 
@@ -67,5 +49,3 @@ BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>("GoalUpdated");
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_
diff --git a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp
index e389133e24411e25f405a34acbc92997b62f4077..41796834c639c5fdf450dca6859ca5185cdde3b5 100644
--- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp
@@ -12,11 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_
-#define NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-
+#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -36,5 +32,3 @@ BT_REGISTER_NODES(factory)
     "InitialPoseReceived",
     std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1));
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_
diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
index 34006cf3fdf31f9a0f3a5c40c08214708f0ac9ae..7fdaca827cec76ebc2eb2b9441529545d9731633 100644
--- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
+++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
@@ -5,3 +5,15 @@ ament_target_dependencies(test_condition_distance_traveled ${dependencies})
 ament_add_gtest(test_condition_time_expired test_time_expired.cpp)
 target_link_libraries(test_condition_time_expired nav2_time_expired_condition_bt_node)
 ament_target_dependencies(test_condition_time_expired ${dependencies})
+
+ament_add_gtest(test_condition_goal_reached test_goal_reached.cpp)
+target_link_libraries(test_condition_goal_reached nav2_goal_reached_condition_bt_node)
+ament_target_dependencies(test_condition_goal_reached ${dependencies})
+
+ament_add_gtest(test_condition_goal_updated test_goal_updated.cpp)
+target_link_libraries(test_condition_goal_updated nav2_goal_updated_condition_bt_node)
+ament_target_dependencies(test_condition_goal_updated ${dependencies})
+
+ament_add_gtest(test_condition_initial_pose_received test_initial_pose_received.cpp)
+target_link_libraries(test_condition_initial_pose_received nav2_initial_pose_received_condition_bt_node)
+ament_target_dependencies(test_condition_initial_pose_received ${dependencies})
diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..820983c08985daee74117bd33496813407200b4e
--- /dev/null
+++ b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp
@@ -0,0 +1,110 @@
+// 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 <chrono>
+#include <memory>
+#include <set>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue{0.1});
+
+    geometry_msgs::msg::PoseStamped goal;
+    goal.header.stamp = node_->now();
+    goal.header.frame_id = "map";
+    goal.pose.position.x = 1.0;
+    goal.pose.position.y = 1.0;
+    config_->blackboard->set("goal", goal);
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <GoalReached goal="{goal}" />
+        </BehaviorTree>
+      </root>)";
+
+    factory_->registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
+    tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  }
+
+  void TearDown()
+  {
+    tree_.reset();
+  }
+
+protected:
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+std::shared_ptr<BT::Tree> GoalReachedConditionTestFixture::tree_ = nullptr;
+
+TEST_F(GoalReachedConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.0;
+  pose.position.y = 0.0;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+
+  pose.position.x = 0.5;
+  pose.position.y = 0.5;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+
+  pose.position.x = 0.9;
+  pose.position.y = 0.9;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);
+
+  pose.position.x = 1.0;
+  pose.position.y = 1.0;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}
diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c2580a29137823a1e86fdaa9311de366ac917469
--- /dev/null
+++ b/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp
@@ -0,0 +1,74 @@
+// 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 <set>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
+
+class GoalUpdatedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedCondition>(
+      "goal_updated", *config_);
+  }
+
+  void TearDown()
+  {
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition> bt_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition>
+GoalUpdatedConditionTestFixture::bt_node_ = nullptr;
+
+TEST_F(GoalUpdatedConditionTestFixture, test_behavior)
+{
+  geometry_msgs::msg::PoseStamped goal;
+  config_->blackboard->set("goal", goal);
+
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}
diff --git a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2070069990f957a88d3df9cc9c73307be7eda36e
--- /dev/null
+++ b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp
@@ -0,0 +1,81 @@
+// 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 <set>
+#include <string>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
+
+class TestNode : public BT::SyncActionNode
+{
+public:
+  TestNode(const std::string & name, const BT::NodeConfiguration & config)
+  : SyncActionNode(name, config)
+  {}
+
+  BT::NodeStatus tick()
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+};
+
+class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    test_node_ = std::make_shared<TestNode>("TestNode", *config_);
+  }
+
+  void TearDown()
+  {
+    test_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<TestNode> test_node_;
+};
+
+std::shared_ptr<TestNode> InitialPoseReceivedConditionTestFixture::test_node_ = nullptr;
+
+TEST_F(InitialPoseReceivedConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::FAILURE);
+  config_->blackboard->set("initial_pose_received", true);
+  EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::SUCCESS);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}