diff --git a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
index 896124807eb9fb8e6e5102357199e1bfee50e90c..b80363a53da8c4a066352f0ed08fa81bb56d43d5 100644
--- a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
+++ b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
@@ -103,7 +103,7 @@ public:
 
     // Enable result awareness by providing an empty lambda function
     auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
-    send_goal_options.result_callback = [](auto){};
+    send_goal_options.result_callback = [](auto) {};
 
 new_goal_received:
     auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
diff --git a/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp b/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp
index e56b59cad5713f978ad83b2a2aaa65c31dbee664..f8751f24fadeeab71f9bc68650b44d47293166a6 100644
--- a/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp
+++ b/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp
@@ -22,7 +22,6 @@
 #include "behaviortree_cpp/condition_node.h"
 #include "nav2_robot/robot.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-//#include "nav2_tasks/compute_path_to_pose_task.hpp"
 
 namespace nav2_tasks
 {
@@ -39,6 +38,7 @@ public:
 
   ~GoalReachedCondition()
   {
+    cleanup();
   }
 
   BT::NodeStatus tick() override
@@ -57,14 +57,17 @@ public:
   {
     node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
     node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
-    //robot_ = std::make_unique<nav2_robot::Robot>(node_);
+    robot_ = std::make_unique<nav2_robot::Robot>(
+      node_->get_node_base_interface(),
+      node_->get_node_topics_interface(),
+      node_->get_node_logging_interface(),
+      true);
     initialized_ = true;
   }
 
   bool
   goalReached()
   {
-#if 0
     auto current_pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
 
     if (!robot_->getCurrentPose(current_pose)) {
@@ -72,25 +75,27 @@ public:
       return false;
     }
     // TODO(mhpanah): replace this with a function
-    blackboard()->get<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("goal", command_);
-    double dx = command_->pose.position.x - current_pose->pose.pose.position.x;
-    double dy = command_->pose.position.y - current_pose->pose.pose.position.y;
+    blackboard()->get<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_);
+    double dx = goal_->pose.position.x - current_pose->pose.pose.position.x;
+    double dy = goal_->pose.position.y - current_pose->pose.pose.position.y;
 
     if ( (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) ) {
       return true;
     } else {
       return false;
     }
-#else
-  return false;
-#endif
+  }
+
+protected:
+  void cleanup()
+  {
+    robot_.reset();
   }
 
 private:
   rclcpp::Node::SharedPtr node_;
   std::unique_ptr<nav2_robot::Robot> robot_;
   geometry_msgs::msg::PoseStamped::SharedPtr goal_;
-  geometry_msgs::msg::PoseStamped::SharedPtr command_;
 
   bool initialized_;
   double goal_reached_tol_;
diff --git a/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
index 9155cacc028ff3dcdfecf910200bfcdb4eb76a09..7049f1af6ffba701bcbdd603e8c0c0e3bfd44a7e 100644
--- a/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
+++ b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
@@ -39,6 +39,7 @@ public:
 
   ~IsLocalizedCondition()
   {
+    cleanup();
   }
 
   BT::NodeStatus tick() override
@@ -51,7 +52,11 @@ public:
       node_->get_parameter_or<double>("is_localized_condition.y_tol", y_tol_, 0.25);
       node_->get_parameter_or<double>("is_localized_condition.rot_tol", rot_tol_, M_PI / 4);
 
-      robot_ = std::make_unique<nav2_robot::Robot>(node_);
+      robot_ = std::make_unique<nav2_robot::Robot>(
+        node_->get_node_base_interface(),
+        node_->get_node_topics_interface(),
+        node_->get_node_logging_interface(),
+        true);
 
       initialized_ = true;
     }
@@ -60,6 +65,11 @@ public:
   }
 
 protected:
+  void cleanup()
+  {
+    robot_.reset();
+  }
+
   bool isLocalized()
   {
     auto current_pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();