diff --git a/smac_planner/include/smac_planner/a_star.hpp b/smac_planner/include/smac_planner/a_star.hpp
index 8234667db71..aa490aaa893 100644
--- a/smac_planner/include/smac_planner/a_star.hpp
+++ b/smac_planner/include/smac_planner/a_star.hpp
@@ -24,17 +24,35 @@
 
 #include "smac_planner/node.hpp"
 #include "smac_planner/types.hpp"
+#include "smac_planner/constants.hpp"
 
 namespace smac_planner
 {
 
 /**
  * @class smac_planner::AStarAlgorithm
- * @brief An A* implementation for planning in a costmap
+ * @brief An A* implementation for planning in a costmap. Templated based on the Node type.
  */
+template<typename NodeT>
 class AStarAlgorithm
 {
 public:
+  typedef NodeT * NodePtr;
+  typedef std::vector<NodeT> Graph;
+  typedef std::vector<NodePtr> NodeVector;
+  typedef std::pair<float, NodePtr> NodeElement;
+  typedef std::pair<float, unsigned int> NodeHeuristicPair;
+
+  struct NodeComparator
+  {
+    bool operator()(const NodeElement & a, const NodeElement & b) const
+    {
+      return a.first > b.first;
+    }
+  };
+
+  typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
+
   /**
    * @brief A constructor for smac_planner::PlannerServer
    * @param neighborhood The type of neighborhood to use for search (4 or 8 connected)
@@ -99,7 +117,7 @@ class AStarAlgorithm
    * @param path Reference to a vector of indicies of generated path
    * @return whether the path was able to be backtraced
    */
-  bool backtracePath(Node * & node, IndexPath & path);
+  bool backtracePath(NodePtr & node, IndexPath & path);
 
   /**
    * @brief Get maximum number of iterations to plan
@@ -112,60 +130,56 @@ class AStarAlgorithm
    * @brief Get pointer reference to starting node
    * @return Node pointer reference to starting node
    */
-  inline Node * & getStart();
+  inline NodePtr & getStart();
 
   /**
    * @brief Get pointer reference to goal node
    * @return Node pointer reference to goal node
    */
-  inline Node * & getGoal();
+  inline NodePtr & getGoal();
 
   /**
    * @brief Get pointer to next goal in open set
    * @return Node pointer reference to next heuristically scored node
    */
-  inline Node * getNode();
+  inline NodePtr getNode();
 
   /**
    * @brief Get pointer to next goal in open set
    * @param cost The cost to sort into the open set of the node
    * @param node Node pointer reference to add to open set
    */
-  inline void addNode(const float cost, Node * & node);
+  inline void addNode(const float cost, NodePtr & node);
 
   /**
-   * @brief Check if this node is the goal node
-   * @param node Node pointer to check if its the goal node
-   * @return if node is goal
+   * @brief Retrieve all valid neighbors of a node.
+   * @param node Pointer to the node we are currently exploring in A*
+   * @param neighbors Vector of neighbors to be filled
    */
-  inline bool isGoal(Node * & node);
+  inline void getNeighbors(NodePtr & node, NodeVector & neighbors);
 
   /**
-   * @brief Check if this node is valid
-   * @param i Node index
-   * @param node_it Iterator reference of the node
-   * @return whether this node is valid and collision free
+   * @brief Initialize the neighborhood to be used in A*
+   * For now we support 4-connect (VON_NEUMANN) and 8-connect (MOORE)
+   * @param x_size The size of the underlying grid
+   * @param neighborhood The desired neighborhood type
    */
-  inline bool isNodeValid(const unsigned int & i, Node * & node_it);
+  inline void initNeighborhoods(const int & x_size, const Neighborhood & neighborhood);
 
   /**
-   * @brief Get a vector of valid node pointers from relative locations
-   * @param lookup_table Lookup table of values around node to query
-   * @param node Node index
-   * @param neighbors Vector of node pointers to valid node
+   * @brief Check if this node is the goal node
+   * @param node Node pointer to check if its the goal node
+   * @return if node is goal
    */
-  inline void getValidNodes(
-    const std::vector<int> & lookup_table,
-    const int & i,
-    NodeVector & neighbors);
+  inline bool isGoal(NodePtr & node);
 
   /**
    * @brief Get cost of traversal between nodes
-   * @param node Node index current
-   * @param node Node index of new
+   * @param current_node Pointer to current node
+   * @param new_node Pointer to new node
    * @return Reference traversal cost between the nodes
    */
-  inline float getTraversalCost(Node * & lastNode, Node * & node);
+  inline float getTraversalCost(NodePtr & current_node, NodePtr & new_node);
 
   /**
    * @brief Get cost of heuristic of node
@@ -175,13 +189,6 @@ class AStarAlgorithm
    */
   inline float getHeuristicCost(const unsigned int & node);
 
-  /**
-   * @brief Get a vector of neighbors around node
-   * @param node Node index
-   * @param neighbors Vector of node pointers to neighbors
-   */
-  inline void getNeighbors(const unsigned int & node, NodeVector & neighbors);
-
   /**
    * @brief Check if inputs to planner are valid
    * @return Are valid
@@ -234,16 +241,14 @@ class AStarAlgorithm
   unsigned int _y_size;
 
   Coordinates _goal_coordinates;
-  Node * _start;
-  Node * _goal;
+  NodePtr _start;
+  NodePtr _goal;
 
   std::unique_ptr<Graph> _graph;
   std::unique_ptr<NodeQueue> _queue;
 
   Neighborhood _neighborhood;
-  std::vector<int> _van_neumann_neighborhood;
-  std::vector<int> _moore_neighborhood;
-
+  std::vector<int> _neighbors_grid_offsets;
   NodeHeuristicPair _best_heuristic_node;
 };
 
diff --git a/smac_planner/include/smac_planner/constants.hpp b/smac_planner/include/smac_planner/constants.hpp
new file mode 100644
index 00000000000..49fe4484e8b
--- /dev/null
+++ b/smac_planner/include/smac_planner/constants.hpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// 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. Reserved.
+
+#ifndef SMAC_PLANNER__CONSTANTS_HPP_
+#define SMAC_PLANNER__CONSTANTS_HPP_
+
+namespace smac_planner
+{
+  enum class Neighborhood
+  {
+    UNKNOWN = 0,
+    VON_NEUMANN = 1,
+    MOORE = 2
+  };
+
+  inline std::string toString(const Neighborhood & n)
+  {
+    switch (n) {
+      case Neighborhood::VON_NEUMANN:
+        return "Von Neumann";
+      case Neighborhood::MOORE:
+        return "Moore";
+      default:
+        return "Unknown";
+    }
+  }
+
+  const float UNKNOWN = 255;
+  const float OCCUPIED = 254;
+  const float INSCRIBED = 253;
+  const float MAX_NON_OBSTACLE = 252;
+  const float FREE = 0;
+
+}  // namespace smac_planner
+
+#endif  // SMAC_PLANNER__CONSTANTS_HPP_
diff --git a/smac_planner/include/smac_planner/node.hpp b/smac_planner/include/smac_planner/node.hpp
index f4dba2c2c1f..0997dcbcf29 100644
--- a/smac_planner/include/smac_planner/node.hpp
+++ b/smac_planner/include/smac_planner/node.hpp
@@ -20,6 +20,8 @@
 #include <queue>
 #include <limits>
 
+#include "smac_planner/constants.hpp"
+
 namespace smac_planner
 {
 
@@ -36,10 +38,10 @@ class Node
    * @param index The index of this node for self-reference
    */
   explicit Node(unsigned char & cost_in, const unsigned int index)
-  : last_node(nullptr),
+  : parent(nullptr),
     _cell_cost(static_cast<float>(cost_in)),
     _accumulated_cost(std::numeric_limits<float>::max()),
-    _i(index),
+    _index(index),
     _was_visited(false),
     _is_queued(false)
   {
@@ -50,7 +52,7 @@ class Node
    */
   ~Node()
   {
-    last_node = nullptr;
+    parent = nullptr;
   }
 
   /**
@@ -60,7 +62,7 @@ class Node
    */
   bool operator==(const Node & rhs)
   {
-    return this->_i == rhs._i;
+    return this->_index == rhs._index;
   }
 
   /**
@@ -70,10 +72,10 @@ class Node
    */
   void reset(const unsigned char & cost, const unsigned int index)
   {
-    last_node = nullptr;
+    parent = nullptr;
     _cell_cost = static_cast<float>(cost);
     _accumulated_cost = std::numeric_limits<float>::max();
-    _i = index;
+    _index = index;
     _was_visited = false;
     _is_queued = false;
   }
@@ -146,15 +148,43 @@ class Node
    */
   unsigned int & getIndex()
   {
-    return _i;
+    return _index;
+  }
+
+  /**
+   * @brief Check if this node is valid
+   * @param traverse_unknown If we can explore unknown nodes on the graph
+   * @return whether this node is valid and collision free
+   */
+  bool isNodeValid(const bool & traverse_unknown) {
+    // NOTE(stevemacenski): Right now, we do not check if the node has wrapped around
+    // the regular grid (e.g. your node is on the edge of the costmap and i+1
+    // goes to the other side). This check would add compute time and my assertion is
+    // that if you do wrap around, the heuristic will be so high it'll be added far
+    // in the queue that it will never be called if a valid path exists.
+    // This is intentionally un-included to increase speed, but be aware. If this causes
+    // trouble, please file a ticket and we can address it then.
+
+    // occupied node
+    auto & cost = this->getCost();
+    if (cost == OCCUPIED || cost == INSCRIBED) {
+      return false;
+    }
+
+    // unknown node
+    if (cost == UNKNOWN && ! traverse_unknown) {
+      return false;
+    }
+
+    return true;
   }
 
-  Node * last_node;
+  Node * parent;
 
 private:
   float _cell_cost;
   float _accumulated_cost;
-  unsigned int _i;
+  unsigned int _index;
   bool _was_visited;
   bool _is_queued;
 };
diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp
index f18dea5c205..9e8bd2b1286 100644
--- a/smac_planner/include/smac_planner/smac_planner.hpp
+++ b/smac_planner/include/smac_planner/smac_planner.hpp
@@ -89,7 +89,7 @@ class SmacPlanner : public nav2_core::GlobalPlanner
   void removeHook(std::vector<Eigen::Vector2d> & path);
 
 protected:
-  std::unique_ptr<AStarAlgorithm> _a_star;
+  std::unique_ptr<AStarAlgorithm<Node>> _a_star;
   std::unique_ptr<Smoother> _smoother;
   std::unique_ptr<Upsampler> _upsampler;
   nav2_util::LifecycleNode::SharedPtr _node;
diff --git a/smac_planner/include/smac_planner/types.hpp b/smac_planner/include/smac_planner/types.hpp
index f829339559b..e2e693394b9 100644
--- a/smac_planner/include/smac_planner/types.hpp
+++ b/smac_planner/include/smac_planner/types.hpp
@@ -16,64 +16,15 @@
 #define SMAC_PLANNER__TYPES_HPP_
 
 #include <vector>
-#include <unordered_map>
-#include <utility>
-#include <queue>
-
-#include "smac_planner/node.hpp"
 
 namespace smac_planner
 {
-
-typedef std::vector<Node> Graph;
-
-typedef std::vector<Node *> NodeVector;
-
 typedef std::vector<unsigned int> IndexPath;
 
 typedef std::pair<float, float> Coordinates;
 
 typedef std::pair<double, double> DoubleCoordinates;
 
-typedef std::pair<float, Node *> NodeElement;
-
-typedef std::pair<float, unsigned int> NodeHeuristicPair;
-
-struct NodeComparator
-{
-  bool operator()(const NodeElement & a, const NodeElement & b) const
-  {
-    return a.first > b.first;
-  }
-};
-
-typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
-
-enum class Neighborhood
-{
-  UNKNOWN = 0,
-  VAN_NEUMANN = 1,
-  MOORE = 2
-};
-
-inline std::string toString(const Neighborhood & n)
-{
-  switch (n) {
-    case Neighborhood::VAN_NEUMANN:
-      return "Van Neumann";
-    case Neighborhood::MOORE:
-      return "Moore";
-    default:
-      return "Unknown";
-  }
-}
-
-const float UNKNOWN = 255;
-const float OCCUPIED = 254;
-const float INSCRIBED = 253;
-const float MAX_NON_OBSTACLE = 252;
-const float FREE = 0;
-
 }  // namespace smac_planner
 
 #endif  // SMAC_PLANNER__TYPES_HPP_
diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp
index 57c8ba09d27..76c4987db97 100644
--- a/smac_planner/src/a_star.cpp
+++ b/smac_planner/src/a_star.cpp
@@ -14,12 +14,8 @@
 
 #include <cmath>
 #include <stdexcept>
-#include <vector>
-#include <unordered_map>
 #include <memory>
-#include <queue>
 #include <algorithm>
-#include <chrono>
 #include <limits>
 
 #include "smac_planner/a_star.hpp"
@@ -27,7 +23,8 @@
 namespace smac_planner
 {
 
-AStarAlgorithm::AStarAlgorithm(const Neighborhood & neighborhood)
+template<typename NodeT>
+AStarAlgorithm<NodeT>::AStarAlgorithm(const Neighborhood & neighborhood)
 : _travel_cost_scale(0.0),
   _neutral_cost(0.0),
   _traverse_unknown(true),
@@ -43,7 +40,8 @@ AStarAlgorithm::AStarAlgorithm(const Neighborhood & neighborhood)
 {
 }
 
-AStarAlgorithm::~AStarAlgorithm()
+template<typename NodeT>
+AStarAlgorithm<NodeT>::~AStarAlgorithm()
 {
   _graph.reset();
   _queue.reset();
@@ -51,7 +49,8 @@ AStarAlgorithm::~AStarAlgorithm()
   _goal = nullptr;
 }
 
-void AStarAlgorithm::initialize(
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::initialize(
   const float & travel_cost_scale,
   const bool & allow_unknown,
   int & max_iterations,
@@ -74,7 +73,8 @@ void AStarAlgorithm::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
 }
 
-void AStarAlgorithm::setCosts(
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::setCosts(
   const unsigned int & x,
   const unsigned int & y,
   unsigned char * & costs)
@@ -83,11 +83,7 @@ void AStarAlgorithm::setCosts(
     _x_size = x;
     _y_size = y;
     int x_size_int = static_cast<int>(_x_size);
-    _van_neumann_neighborhood = {
-      -1, +1, -x_size_int, +x_size_int};
-    _moore_neighborhood = {
-      -1, +1, -x_size_int, +x_size_int, -x_size_int - 1,
-      -x_size_int + 1, +x_size_int - 1, +x_size_int + 1};
+    initNeighborhoods(x_size_int, _neighborhood);
     _graph->clear();
     _graph->reserve(x * y);
     for (unsigned int i = 0; i != x * y; i++) {
@@ -101,18 +97,41 @@ void AStarAlgorithm::setCosts(
   }
 }
 
-void AStarAlgorithm::setStart(const unsigned int & value)
+template <typename NodeT>
+void AStarAlgorithm<NodeT>::initNeighborhoods(
+  const int & x_size,
+  const Neighborhood & neighborhood)
+{
+  switch (neighborhood) {
+    case Neighborhood::UNKNOWN:
+      throw std::runtime_error("Unknown neighborhood type selected.");
+    case Neighborhood::VON_NEUMANN:
+      _neighbors_grid_offsets = {-1, +1, -x_size, +x_size};
+      break;
+    case Neighborhood::MOORE:
+      _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1,
+                                 -x_size + 1, +x_size - 1, +x_size + 1};
+      break;
+    default:
+      throw std::runtime_error("Invalid neighborhood type selected.");
+  }
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::setStart(const unsigned int & value)
 {
   _start = & _graph->operator[](value);
 }
 
-void AStarAlgorithm::setGoal(const unsigned int & value)
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::setGoal(const unsigned int & value)
 {
   _goal = & _graph->operator[](value);
   _goal_coordinates = getCoords(_goal->getIndex());
 }
 
-bool AStarAlgorithm::areInputsValid()
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::areInputsValid()
 {
   // Check if initialization was called
   if (!_graph) {
@@ -120,7 +139,7 @@ bool AStarAlgorithm::areInputsValid()
   }
 
   // Check if graph was filled in
-  if (_graph->size() == 0) {
+  if (_graph->empty()) {
     throw std::runtime_error("Failed to compute path, no costmap given.");
   }
 
@@ -130,20 +149,20 @@ bool AStarAlgorithm::areInputsValid()
   }
 
   // Check if ending point is valid
-  Node * node;
-  if (getToleranceHeuristic() < 0.001 && !isNodeValid(_goal->getIndex(), node)) {
+  if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown)) {
     throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance.");
   }
 
   // Check if starting point is valid
-  if (!isNodeValid(getStart()->getIndex(), node)) {
+  if (!_start->isNodeValid(_traverse_unknown)) {
     throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan.");
   }
 
   return true;
 }
 
-bool AStarAlgorithm::createPath(IndexPath & path, int & iterations, const float & tolerance)
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::createPath(IndexPath & path, int & iterations, const float & tolerance)
 {  
   if (!areInputsValid()) {
     return false;
@@ -158,11 +177,11 @@ bool AStarAlgorithm::createPath(IndexPath & path, int & iterations, const float
   getStart()->setAccumulatedCost(0.0);
 
   // Optimization: preallocate all variables
-  Node * current_node;
+  NodePtr current_node;
   float g_cost;
   NodeVector neighbors;
   int approach_iterations = 0;
-  NodeVector::iterator neighbor_iterator;
+  typename NodeVector::iterator neighbor_iterator;
 
   while (iterations < getMaxIterations() && !_queue->empty()) {
 
@@ -190,19 +209,19 @@ bool AStarAlgorithm::createPath(IndexPath & path, int & iterations, const float
       if (approach_iterations > getOnApproachMaxIterations() ||
         iterations + 1 == getMaxIterations())
       {
-        Node * node = & _graph->operator[](_best_heuristic_node.second);
+        NodePtr node = & _graph->operator[](_best_heuristic_node.second);
         return backtracePath(node, path);
       }
     }
 
     // 4) Expand neighbors of Nbest not visited
     neighbors.clear();
-    getNeighbors(current_node->getIndex(), neighbors);
+    getNeighbors(current_node, neighbors);
 
     for (neighbor_iterator = neighbors.begin();
       neighbor_iterator != neighbors.end(); ++neighbor_iterator)
     {
-      Node * & neighbor = * neighbor_iterator;
+      NodePtr & neighbor = * neighbor_iterator;
 
       // 4.1) Compute the cost to go to this node
       g_cost = current_node->getAccumulatedCost() +
@@ -211,7 +230,7 @@ bool AStarAlgorithm::createPath(IndexPath & path, int & iterations, const float
       // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach
       if (g_cost < neighbor->getAccumulatedCost()) {
         neighbor->setAccumulatedCost(g_cost);
-        neighbor->last_node = current_node;
+        neighbor->parent = current_node;
 
         // 4.3) If not in queue or visited, add it
         if (!neighbor->wasVisited()) {
@@ -225,65 +244,101 @@ bool AStarAlgorithm::createPath(IndexPath & path, int & iterations, const float
   return false;
 }
 
-bool AStarAlgorithm::isGoal(Node * & node)
+// Specialized method getNeighbors for 2D nodes
+template <>
+void AStarAlgorithm<Node>::getNeighbors(NodePtr & node, NodeVector & neighbors)
+{
+  // NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free
+  // space and then expand 8-connected, the first set of neighbors will be all cost
+  // _neutral_cost. Then its expansion will all be 2 * _neutral_cost but now multiple
+  // nodes are touching that node so the last cell to update the back pointer wins.
+  // Thusly, the ordering ends with the cardinal directions for both sets such that
+  // behavior is consistent in large free spaces between them.
+  // 100  50   0
+  // 100  50  50
+  // 100 100 100   where lower-middle '100' is visited with same cost by both bottom '50' nodes
+  // Therefore, it is valuable to have some low-potential across the entire map
+  // rather than a small inflation around the obstacles
+  int index;
+  NodePtr neighbor;
+  int node_i = node->getIndex();
+
+  for(unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) {
+    index = node_i + _neighbors_grid_offsets[i];
+    if (index > 0)
+    {
+      neighbor = & _graph->operator[](index);
+      if (neighbor->isNodeValid(_traverse_unknown))
+      {
+        neighbors.push_back(neighbor);
+      }
+    }
+  }
+}
+
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
 {
   return node == getGoal();
 }
 
-bool AStarAlgorithm::backtracePath(Node * & node, IndexPath & path)
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::backtracePath(NodePtr & node, IndexPath & path)
 {
-  if (!node->last_node) {
+  if (!node->parent) {
     return false;
   }
 
-  Node * current_node = node;
+  NodePtr current_node = node;
 
-  while (current_node->last_node) {
+  while (current_node->parent) {
     path.push_back(current_node->getIndex());
-    current_node = current_node->last_node;
-  }
-
-  if (path.size() > 1) {
-    return true;
+    current_node = current_node->parent;
   }
 
-  return false;
+  return path.size() > 1;
 }
 
-Node * & AStarAlgorithm::getStart()
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
 {
   return _start;
 }
 
-Node * & AStarAlgorithm::getGoal()
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal()
 {
   return _goal;
 }
 
-Node * AStarAlgorithm::getNode()
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getNode()
 {
-  Node * node = _queue->top().second;
+  NodePtr node = _queue->top().second;
   _queue->pop();
   return node;
 }
 
-void AStarAlgorithm::addNode(const float cost, Node * & node)
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::addNode(const float cost, NodePtr & node)
 {
   _queue->emplace(cost, node);
 }
 
-float AStarAlgorithm::getTraversalCost(
-  Node * & last_node,
-  Node * & node)
+template<typename NodeT>
+float AStarAlgorithm<NodeT>::getTraversalCost(
+  NodePtr & current_node,
+  NodePtr & new_node)
 {
-  float & node_cost = node->getCost();
+  float & node_cost = new_node->getCost();
 
   // rescale cost quadratically, makes search more convex
   // Higher the scale, the less cost for lengthwise expansion
   return _neutral_cost + _travel_cost_scale * node_cost * node_cost;
 }
 
-float AStarAlgorithm::getHeuristicCost(const unsigned int & node)
+template<typename NodeT>
+float AStarAlgorithm<NodeT>::getHeuristicCost(const unsigned int & node)
 {
   Coordinates node_coords = getCoords(node);
   float heuristic = hypotf(
@@ -302,115 +357,54 @@ float AStarAlgorithm::getHeuristicCost(const unsigned int & node)
   return heuristic;
 }
 
-void AStarAlgorithm::getNeighbors(const unsigned int & node, NodeVector & neighbors)
-{
-  // NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free
-  // space and then expand 8-connected, the first set of neighbors will be all cost
-  // _neutral_cost. Then its expansion will all be 2 * _neutral_cost but now multiple
-  // nodes are touching that node so the last cell to update the back pointer wins.
-  // Thusly, the ordering ends with the cardinal directions for both sets such that
-  // behavior is consistent in large free spaces between them. 
-  // 100  50   0 
-  // 100  50  50
-  // 100 100 100   where lower-middle '100' is visited with same cost by both bottom '50' nodes
-  // Therefore, it is valuable to have some low-potential across the entire map
-  // rather than a small inflation around the obstacles
-  const int node_i = static_cast<int>(node);
-
-  switch (_neighborhood) {
-    case Neighborhood::UNKNOWN:
-      throw std::runtime_error("Unkown neighborhood type selected.");
-    case Neighborhood::VAN_NEUMANN:
-      getValidNodes(_van_neumann_neighborhood, node_i, neighbors);
-      break;
-    case Neighborhood::MOORE:
-      getValidNodes(_moore_neighborhood, node_i, neighbors);
-      break;
-    default:
-      throw std::runtime_error("Invalid neighborhood type selected.");
-  }
-}
-
-void AStarAlgorithm::getValidNodes(
-  const std::vector<int> & lookup_table,
-  const int & node_i,
-  NodeVector & neighbors)
-{
-  Node * node;
-  int index = 0;
-
-  for (unsigned int i = 0; i != lookup_table.size(); i++) {
-    index = node_i + lookup_table[i];
-    if (index > 0 && isNodeValid(index, node))
-    {
-      neighbors.push_back(node);
-    }
-  }
-}
-
-bool AStarAlgorithm::isNodeValid(const unsigned int & i, Node * & node)
-{
-  node = & _graph->operator[](i);
-
-  // NOTE(stevemacenski): Right now, we do not check if the node has wrapped around
-  // the regular grid (e.g. your node is on the edge of the costmap and i+1 
-  // goes to the other side). This check would add compute time and my assertion is
-  // that if you do wrap around, the heuristic will be so high it'll be added far
-  // in the queue that it will never be called if a valid path exists.
-  // This is intentionally un-included to increase speed, but be aware. If this causes
-  // trouble, please file a ticket and we can address it then.
-
-  // occupied node
-  auto & cost = node->getCost();
-  if (cost == OCCUPIED || cost == INSCRIBED) {
-    return false;
-  }
-
-  // unknown node
-  if (cost == UNKNOWN && !_traverse_unknown) {
-    return false;
-  }
-
-  return true;
-}
-
-void AStarAlgorithm::clearQueue()
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::clearQueue()
 {
   while (!_queue->empty()) {
     _queue->pop();
   }
 }
 
-Coordinates AStarAlgorithm::getCoords(const unsigned int & index)
+template<typename NodeT>
+Coordinates AStarAlgorithm<NodeT>::getCoords(const unsigned int & index)
 {
   return Coordinates(
     static_cast<float>(index % getSizeX()),
     static_cast<float>(index / getSizeX()));
 }
 
-int & AStarAlgorithm::getMaxIterations()
+template<typename NodeT>
+int & AStarAlgorithm<NodeT>::getMaxIterations()
 {
   return _max_iterations;
 }
 
-int & AStarAlgorithm::getOnApproachMaxIterations()
+template<typename NodeT>
+int & AStarAlgorithm<NodeT>::getOnApproachMaxIterations()
 {
   return _max_on_approach_iterations;
 }
 
-float & AStarAlgorithm::getToleranceHeuristic()
+template<typename NodeT>
+float & AStarAlgorithm<NodeT>::getToleranceHeuristic()
 {
   return _tolerance;
 }
 
-unsigned int & AStarAlgorithm::getSizeX()
+template<typename NodeT>
+unsigned int & AStarAlgorithm<NodeT>::getSizeX()
 {
   return _x_size;
 }
 
-unsigned int & AStarAlgorithm::getSizeY()
+template<typename NodeT>
+unsigned int & AStarAlgorithm<NodeT>::getSizeY()
 {
   return _y_size;
 }
 
+// Instantiate AStartAlgorithm for the supported template type parameters
+// This is needed to prevent "undefined symbol" errors at runtime.
+template class AStarAlgorithm<Node>;
+
 }  // namespace smac_planner
diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp
index 36e1febc844..bb18b08eb40 100644
--- a/smac_planner/src/smac_planner.cpp
+++ b/smac_planner/src/smac_planner.cpp
@@ -135,13 +135,13 @@ void SmacPlanner::configure(
   Neighborhood neighborhood;
   if (neighborhood_for_search == std::string("MOORE")) {
     neighborhood = Neighborhood::MOORE;
-  } else if (neighborhood_for_search == std::string("VAN_NEUMANN")) {
-    neighborhood = Neighborhood::VAN_NEUMANN;
+  } else if (neighborhood_for_search == std::string("VON_NEUMANN")) {
+    neighborhood = Neighborhood::VON_NEUMANN;
   } else {
     neighborhood = Neighborhood::MOORE;
     RCLCPP_WARN(_node->get_logger(),
       "Unable to get Neighborhood search type. Given '%s', "
-      "valid options are MOORE and VAN_NEUMANN. Using MOORE as default",
+      "valid options are MOORE and VON_NEUMANN. Using MOORE as default",
       neighborhood_for_search.c_str());
   }
 
@@ -168,7 +168,7 @@ void SmacPlanner::configure(
     _upsampling_ratio = 2;
   }
 
-  _a_star = std::make_unique<AStarAlgorithm>(neighborhood);
+  _a_star = std::make_unique<AStarAlgorithm<Node>>(neighborhood);
   _a_star->initialize(
     travel_cost_scale,
     allow_unknown,
@@ -434,8 +434,8 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
 #ifdef BENCHMARK_TESTING
   steady_clock::time_point b = steady_clock::now();
   duration<double> time_span = duration_cast<duration<double> >(b-a);
-  cout << "It took " << time_span.count() <<
-    " seconds with " << num_iterations << " iterations." <<  endl;
+  cout << "It took " << time_span.count() * 1000 <<
+    " milliseconds with " << num_iterations << " iterations." <<  endl;
 #endif
 
   return plan;