diff --git a/CMakeLists.txt b/CMakeLists.txt
index d6c4e13..0d59388 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -66,14 +66,26 @@ install(
 )
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(${PROJECT_NAME}_my_test test/test.cpp)
+
+  # Link isc_nav files
+  target_include_directories(${PROJECT_NAME}_my_test PUBLIC src/)
+
+  target_include_directories(${PROJECT_NAME}_my_test PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+  # We need to ament link because we use isc_nav files which need ros
+  ament_target_dependencies(
+  ${PROJECT_NAME}_my_test
+  geometry_msgs
+  rclcpp
+  nav_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+)
 endif()
 
 ament_package()
diff --git a/include/isc_nav/pure_pursuit.hpp b/include/isc_nav/pure_pursuit.hpp
index 0ad785f..4600ceb 100644
--- a/include/isc_nav/pure_pursuit.hpp
+++ b/include/isc_nav/pure_pursuit.hpp
@@ -27,12 +27,14 @@
 #include <vector>
 #include "utility/point.hpp"
 
+
+
+namespace isc_nav
+{
 typedef std::vector<Point3D> Path;
 typedef std::pair<Point3D, Point3D> Segment3D;
 typedef std::pair<Point2D, Point2D> Segment2D;
 
-namespace PurePursuit
-{
 class PurePursuit 
 {
 public:
diff --git a/include/isc_nav/pure_pursuit_node.hpp b/include/isc_nav/pure_pursuit_node.hpp
index 55273a7..4469c3d 100644
--- a/include/isc_nav/pure_pursuit_node.hpp
+++ b/include/isc_nav/pure_pursuit_node.hpp
@@ -26,7 +26,7 @@ namespace isc_nav
 class PurePursuitNode : public rclcpp::Node
 {
 public:
-  PurePursuit::PurePursuit m_tracker;
+  PurePursuit m_tracker;
   explicit PurePursuitNode(rclcpp::NodeOptions options);
 
 private:
diff --git a/include/isc_nav/utility/bfs.hpp b/include/isc_nav/utility/bfs.hpp
index 59cf7fb..55fdb93 100644
--- a/include/isc_nav/utility/bfs.hpp
+++ b/include/isc_nav/utility/bfs.hpp
@@ -34,7 +34,7 @@ class BreadthFirstSearch
             // TODO: Param
             if ((current - goal_).x < 0.1 && (current - goal_).y < 0.1)
             {
-                std::cout << "Goal found, getting path." << std::endl;
+                //std::cout << "Goal found, getting path." << std::endl;
                 trace_back_path(current, path, came_from);
             }
 
diff --git a/include/isc_nav/utility/distance_helper.hpp b/include/isc_nav/utility/distance_helper.hpp
index b6ab31d..40c76bd 100644
--- a/include/isc_nav/utility/distance_helper.hpp
+++ b/include/isc_nav/utility/distance_helper.hpp
@@ -3,6 +3,9 @@
 #include "point.hpp"
 #include <math.h>
 
+namespace isc_nav
+{
+
 constexpr double EPSILON = 1e-3;
 
 // distance formula function that finds distance between two points
@@ -46,3 +49,4 @@ inline bool approximately_equals( const Point2D& expected, const Point2D& actual
 	return ( std::fabs( actual.x - expected.x ) < EPSILON )
 		&& ( std::fabs( actual.y - expected.y ) < EPSILON );
 }
+}
\ No newline at end of file
diff --git a/include/isc_nav/utility/map.hpp b/include/isc_nav/utility/map.hpp
index 084d109..4b2f0ef 100644
--- a/include/isc_nav/utility/map.hpp
+++ b/include/isc_nav/utility/map.hpp
@@ -1,8 +1,10 @@
 #pragma once
 
+#include <cstdint>
 #include <inttypes.h>
-#include <isc_nav/utility/point.hpp>
+#include "../utility/point.hpp"
 #include <nav_msgs/msg/occupancy_grid.hpp>
+#include "utility"
 
 namespace isc_nav
 {
@@ -71,7 +73,7 @@ class CostMap
 
     bool valid_cell(const int32_t& x, const int32_t& y) const noexcept
     {
-        if (x > size_x_ || y > size_y_ ||
+        if (x > (int64_t)size_x_ || y > (int64_t)size_y_ ||
             x < 0       || y < 0)
         {
             return false;
@@ -132,13 +134,9 @@ class CostMap
         return neighbors;
     }
 
-private:
-    uint32_t size_x_;
-    uint32_t size_y_;
-    float resolution_;
-    Point2D origin_;
-
-    std::vector<uint8_t> costmap_;
+    double get_size_x() { return size_x_; }
+    double get_size_y() { return size_y_; }
+    float get_resolution() { return resolution_; }
 
     // Values cooresponding to definition of Occupancy Grid in ROS
     static constexpr int8_t OCC_GRID_UNKNOWN = -1;
@@ -149,5 +147,13 @@ class CostMap
     static constexpr uint8_t NO_INFORMATION = 255;
     static constexpr uint8_t LETHAL_OBSTACLE = 254;
     static constexpr uint8_t FREE_SPACE = 0;
+    
+private:
+    uint32_t size_x_;
+    uint32_t size_y_;
+    float resolution_;
+    Point2D origin_;
+
+    std::vector<uint8_t> costmap_;
 };
 } // namespace isc_nav
diff --git a/include/isc_nav/utility/point.hpp b/include/isc_nav/utility/point.hpp
index 485ee65..a4aac13 100644
--- a/include/isc_nav/utility/point.hpp
+++ b/include/isc_nav/utility/point.hpp
@@ -3,6 +3,8 @@
 #include <tuple>
 #include <vector>
 
+namespace isc_nav
+{
 /**
  * @brief data structure to represent x and y coordinates for the robot
  */
@@ -55,12 +57,13 @@ inline bool operator!=(const Point3D& lhs, const Point3D& rhs)
 {
     return ( lhs.x != rhs.x && lhs.y != rhs.y && lhs.z != rhs.z );
 }
+}
 
 namespace std
 {
-template<> struct hash<Point2D>
+template<> struct hash<isc_nav::Point2D>
 {
-    [[nodiscard]] std::size_t operator()(const Point2D& point) const noexcept
+    [[nodiscard]] std::size_t operator()(const isc_nav::Point2D& point) const noexcept
     {
         return std::hash<size_t>()(static_cast<size_t>(point.x) ^ (static_cast<size_t>(point.y) << 16));
     }
diff --git a/include/isc_nav/utility/rrt.hpp b/include/isc_nav/utility/rrt.hpp
new file mode 100644
index 0000000..c5758b9
--- /dev/null
+++ b/include/isc_nav/utility/rrt.hpp
@@ -0,0 +1,342 @@
+#pragma once
+
+#include <isc_nav/utility/point.hpp>
+#include <cmath>
+#include <isc_nav/utility/map.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <string>
+#include <vector>
+#include <cstdlib>
+#include <ctime>
+#include <random>
+
+namespace isc_nav
+{
+    class RRT 
+    {
+    public:
+        /**
+        * @brief Construct a new RRT object and seed the number generator
+        * @param grid occupancy grid
+        */
+        //RRT();
+        RRT(const nav_msgs::msg::OccupancyGrid& grid) : start_{}, goal_{}, map_(grid) {
+            std::random_device rd;
+            std::mt19937 rng(rd());
+            
+            this->rng = rng;
+        }
+
+        /**
+         * @brief Get path to start rrt
+         * @param start_pose a 2D point
+         * @param goal_pose a 2D point
+         * @return const nav_msgs::msg::Path 
+         */
+        const nav_msgs::msg::Path get_path(const geometry_msgs::msg::Pose& start_pose, const geometry_msgs::msg::Pose& goal_pose)
+        {
+            nav_msgs::msg::Path path;
+            start_ = Point2D(start_pose.position.x, start_pose.position.y);
+            goal_ = Point2D(goal_pose.position.x, goal_pose.position.y);
+
+            Point2D random{}, Xnew{}, Xnearest{};
+            double Xnearest_dist = 0;
+
+            // Graph with vertices and edges
+            V.push_back(start_);
+            E.push_back(0);
+            G.push_back(E);
+
+            for (int i = 0; i < lim_; i++)
+            {   
+                random = randomPosition();
+
+                Xnearest = findNearest(random, Xnearest_dist);
+
+                if (getDistance(Xnearest, random) <= max_dist_)
+                {
+                    // If the random node is within the max_dist_ set it as the new node
+                    Xnew = Xnearest;
+                }
+                else
+                {   
+                    Xnew = findNew(Xnearest, random, Xnearest_dist);
+                }
+
+                if (isInObstacle(Xnew))
+                {
+                    continue; // Skip this loop
+                }
+                
+                if (!isCollisionFree(Xnearest, Xnew))
+                {
+                    continue; // Skip this loop
+                }
+
+                // Get the index of new random node and the nearest vertex
+                int attach_to = getIndex(Xnew);
+                int attach_from = getIndex(Xnearest);
+
+                if (attach_from == -1) {
+                    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Xnearest does not exist");
+                    continue; // Skip this loop
+                }
+
+                if (attach_to == -1)
+                {
+                    int add_new = V.size();
+                    V.push_back(Xnew);
+                    E.push_back(0);
+
+                    for (int i = 0; i < add_new; i++)
+                    {
+                        // Fill in the new column
+                        G.at(i).push_back(0);
+                    }
+
+                    // Fill in new row
+                    G.push_back(E);
+                    // Add edge
+                    G.at(attach_from).at(add_new)++;
+                }
+                else 
+                {
+                    // Add edge
+                    G.at(attach_from).at(attach_to)++;
+                }
+
+                // If vertex at goal return graph
+                if ((Xnew.x - goal_.x < 0.1) && (Xnew.y - goal_.y < 0.1))
+                {
+                    trace_back_path(Xnew, path);
+                }
+            }
+
+            return path;
+        }
+
+        /**
+         * @brief Generate a random position on the map
+         * @return Point2D 
+         */
+        Point2D randomPosition()
+        {   
+            std::uniform_real_distribution<double> dist_x(0.0, map_.get_size_x());
+            std::uniform_real_distribution<double> dist_y(0.0, map_.get_size_y());
+            while (true) 
+            {
+                double x = dist_x(rng);
+                double y = dist_y(rng);
+
+                // Check if the generated random node is on the map
+                if (map_.valid_cell(x, y)) 
+                {
+                    Point2D random_node(x, y);
+                    return random_node;
+                }
+            }
+        }
+
+        /**
+         * @brief Get the vertex (already generated) nearest to the random node
+         * @param V a list of generated vertices
+         * @param random_node 
+         * @param Xnearest_dist track the distance between nearest vertex and random node
+         * @return Point2D 
+         */
+        Point2D findNearest(const Point2D& random_node, double& Xnearest_dist)
+        {
+            double dist{};
+            Point2D Xnearest{};
+
+            if (!V.empty()) 
+            {
+                // Assign values to be compared
+                Xnearest_dist = getDistance(V.at(0), random_node);
+                Xnearest = V.at(0);
+            }
+            else {
+                RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Empty list");
+                return Xnearest;
+            }
+
+            for (auto vertex : V)
+            {
+                dist = getDistance(vertex, random_node);
+                if (dist < Xnearest_dist)
+                {   
+                    // Update distance between nearest vertex and random node
+                    Xnearest_dist = dist;
+                    Xnearest = vertex;
+                }
+            }
+
+            return Xnearest;
+        }
+
+        /**
+         * @brief Get a new node that is on the line (connecting the nearest vertex to random node) 
+                    and within the max_dist_ to random node
+         * @param p1 nearest vertex (already in the generated list)
+         * @param p2 random node
+         * @param Xnearest_dist track the distance between nearest vertex and random node
+         * @return Point2D 
+         */
+        Point2D findNew(const Point2D& p1, const Point2D& p2, const double& Xnearest_dist)
+        {
+            Point2D Xnew{};
+            double n = max_dist_;
+            double m = Xnearest_dist - n;
+
+            Xnew.x = (m * p2.x + n * p1.x)/(m + n);
+            Xnew.y = (m * p2.y + n * p1.y)/(m + n);
+
+            return Xnew;
+        }
+
+        /**
+         * @brief Check if the new random node is at obstacle on map
+         * @param random_node a random generated nose
+         * @return true 
+         * @return false 
+         */
+        bool isInObstacle(const Point2D& random_node)
+        {
+            return map_.at(random_node) == CostMap::LETHAL_OBSTACLE;
+        }
+
+        /**
+         * @brief Check if there is any obstacle between the nearest vertex and random node
+         * @param p1 nearest node
+         * @param p2 random node (aka new node)
+         * @return true 
+         * @return false 
+         */
+        bool isCollisionFree(const Point2D& p1, const Point2D& p2)
+        {
+            float reso = map_.get_resolution();
+            double dist = getDistance(p1, p2);
+            int num_of_pt = static_cast<int>(dist / reso);
+            Point2D pt = p1;
+
+            for(int i = 0; i < num_of_pt; i++)
+            {
+                pt = generateAlongLine(pt, p2, dist, reso);
+                if (isInObstacle(pt))
+                {
+                    return false;
+                }
+            }
+            
+            return true;
+        }
+
+        /**
+         * @brief Generate a node on the line connecting nearest vertex and new random node
+         * @param p1 new generated node along the line
+         * @param p2 new random node
+         * @param dist distance between the new generated along the line and the random node
+         * @param reso resolution of the map
+         * @return Point2D 
+         */
+        Point2D generateAlongLine(const Point2D& p1, const Point2D& p2, double& dist, const float& reso)
+        {
+            Point2D Xnew{};
+            double n = reso;
+            double m = dist - n;
+
+            Xnew.x = (m * p2.x + n * p1.x)/(m + n);
+            Xnew.y = (m * p2.y + n * p1.y)/(m + n);
+
+            dist -= reso;
+            return Xnew;
+        }
+
+        /**
+         * @brief Get the distance between 2 points on map
+         * @param p1 a 2D point
+         * @param p2 a 2D point
+         * @return double 
+         */
+        double getDistance(const Point2D& p1, const Point2D& p2)
+        {
+            double dist = std::sqrt(pow((p1.x + p2.x), 2) + pow((p1.y + p2.y), 2));
+            return dist;
+        }
+
+        /**
+         * @brief Get the index of a point in the vector list (which corresponds to graph G)
+         * @param v 
+         * @param key 
+         * @return int 
+         */
+        int getIndex(const Point2D& key)
+        {
+            auto it = find(V.begin(), V.end(), key);
+        
+            // If element was found
+            if (it != V.end())
+            {
+                // Calculate the index for key
+                int index = it - V.begin();
+                return index;
+            }
+            else {
+                // If the element is not present in the vector
+                return -1;
+            }
+        }
+
+        /**
+         * @brief Generate a path from the graph
+         * @param current the latest generated random node that falls in the goal
+         * @param path
+         */
+        void trace_back_path(const Point2D& end, nav_msgs::msg::Path& path) noexcept
+        {
+            int size = V.size();
+            Point2D current = end;
+            int curr_index = getIndex(current);
+
+            while (current != start_)
+            {
+                geometry_msgs::msg::PoseStamped pose{};
+                pose.pose.position.x = current.x;
+                pose.pose.position.y = current.y;
+                path.poses.emplace_back(pose);
+                
+                for (int i = 0; i < size; i++)
+                {
+                    if (G.at(i).at(curr_index))
+                    {
+                        current = V.at(i);
+                        break;
+                    }
+                }
+            }
+
+            std::reverse(path.poses.begin(), path.poses.end());
+        }
+
+    private:
+        Point2D start_;
+        Point2D goal_;
+        CostMap map_;
+
+        std::vector<std::vector<int>> G;
+        std::vector<int> E;
+        std::vector<Point2D> V;
+
+        // Constants
+        double max_dist_ = 50; // Shld be shorter of longer??
+        int lim_ = 1000; // Number of iterations
+
+        // Random number generator
+        std::mt19937 rng;
+    };
+
+} // namespace isc_nav
\ No newline at end of file
diff --git a/package.xml b/package.xml
index d842c6a..962cbee 100644
--- a/package.xml
+++ b/package.xml
@@ -16,8 +16,7 @@
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/src/path_planner_node.cpp b/src/path_planner_node.cpp
index 96b5c99..d64cba2 100644
--- a/src/path_planner_node.cpp
+++ b/src/path_planner_node.cpp
@@ -20,8 +20,9 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
 
-#include "isc_nav/path_planner_node.hpp"
+#include <isc_nav/path_planner_node.hpp>
 #include <isc_nav/utility/bfs.hpp>
+#include <isc_nav/utility/rrt.hpp>
 
 namespace isc_nav
 {
@@ -33,7 +34,7 @@ PathPlanner::PathPlanner(rclcpp::NodeOptions options)
 {
     this->declare_parameter<std::string>("robot_frame", "base_footprint");
     this->declare_parameter<std::string>("map_frame", "map");
-    this->declare_parameter<float>("tf_timeout", 0.03f);
+    this->declare_parameter<float>("tf_timeout", 0.5f);
     update_params();
     param_update_timer_ = this->create_wall_timer(
       1000ms, std::bind(&PathPlanner::update_params, this)
@@ -90,12 +91,22 @@ void PathPlanner::update_plan()
 
     RCLCPP_INFO(this->get_logger(), "Updating plan.");
 
+    /*
     auto bfs = BreadthFirstSearch(*last_map_state_);
     nav_msgs::msg::Path bfs_path = bfs.get_path(*last_pos_state_, last_goal_state_->pose);
     
     bfs_path.header.frame_id = map_frame_;
     bfs_path.header.stamp = this->get_clock()->now();
     path_publisher_->publish(bfs_path);
+    */
+
+    auto rrt = RRT(*last_map_state_);
+    nav_msgs::msg::Path rrt_path = rrt.get_path(*last_pos_state_, last_goal_state_->pose);
+    
+    rrt_path.header.frame_id = map_frame_;
+    rrt_path.header.stamp = this->get_clock()->now();
+    path_publisher_->publish(rrt_path);
+
 }
 
 void PathPlanner::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
diff --git a/src/pure_pursuit.cpp b/src/pure_pursuit.cpp
index 48dd576..b9d4e62 100644
--- a/src/pure_pursuit.cpp
+++ b/src/pure_pursuit.cpp
@@ -29,7 +29,7 @@
 #include <limits>
 #include <stdexcept>
 
-namespace PurePursuit
+namespace isc_nav
 {
 
 PurePursuit::PurePursuit(const Path& robot_path, const double& lookahead_distance): m_robot_path(robot_path)