diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h
index 4f71a42d0..c8f9c08c3 100644
--- a/local_planner/include/local_planner/tree_node.h
+++ b/local_planner/include/local_planner/tree_node.h
@@ -15,6 +15,7 @@ class TreeNode {
   float heuristic_;
   int origin_;
   bool closed_;
+  int depth_;
 
   TreeNode();
   TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel);
diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp
index e5e3050e7..15abe76c4 100644
--- a/local_planner/src/nodes/star_planner.cpp
+++ b/local_planner/src/nodes/star_planner.cpp
@@ -60,8 +60,6 @@ void StarPlanner::buildLookAheadTree() {
   for (int n = 0; n < n_expanded_nodes_ && is_expanded_node; n++) {
     Eigen::Vector3f origin_position = tree_[origin].getPosition();
     Eigen::Vector3f origin_velocity = tree_[origin].getVelocity();
-    PolarPoint facing_goal = cartesianToPolarHistogram(goal_, origin_position);
-    float distance_to_goal = (goal_ - origin_position).norm();
 
     histogram.setZero();
     generateNewHistogram(histogram, cloud_, origin_position);
@@ -100,6 +98,7 @@ void StarPlanner::buildLookAheadTree() {
           float h = treeHeuristicFunction(tree_.size() - 1);
           tree_.back().heuristic_ = h;
           tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + candidate.cost + h;
+          tree_.back().depth_ = tree_[origin].depth_ + 1;
           children++;
         }
       }
@@ -125,8 +124,21 @@ void StarPlanner::buildLookAheadTree() {
     cost_image_data.clear();
     candidate_vector.clear();
   }
-  // smoothing between trees
-  int tree_end = origin;
+
+  // find best node to follow, taking into account A* completion
+  int max_depth = 0;
+  int max_depth_index = 0;
+  for (size_t i = 0; i < tree_.size(); i++) {
+    if (!(tree_[i].closed_)) {
+      if (tree_[i].depth_ > max_depth) {
+        max_depth = tree_[i].depth_;
+        max_depth_index = i;
+      }
+    }
+  }
+
+  // build final tree
+  int tree_end = max_depth_index;
   path_node_positions_.clear();
   while (tree_end > 0) {
     path_node_positions_.push_back(tree_[tree_end].getPosition());
diff --git a/local_planner/src/nodes/tree_node.cpp b/local_planner/src/nodes/tree_node.cpp
index 7e1577804..7afd4838f 100644
--- a/local_planner/src/nodes/tree_node.cpp
+++ b/local_planner/src/nodes/tree_node.cpp
@@ -2,13 +2,13 @@
 
 namespace avoidance {
 
-TreeNode::TreeNode() : total_cost_{0.0f}, heuristic_{0.0f}, origin_{0}, closed_{false} {
+TreeNode::TreeNode() : total_cost_{0.0f}, heuristic_{0.0f}, origin_{0}, closed_{false}, depth_(0) {
   position_ = Eigen::Vector3f::Zero();
   velocity_ = Eigen::Vector3f::Zero();
 }
 
 TreeNode::TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel)
-    : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false} {
+    : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false}, depth_(0) {
   position_ = pos;
   velocity_ = vel;
 }