diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..2d2dcbf
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+# ignore compiled byte code
+*.pyc
+*.npy
\ No newline at end of file
diff --git a/Dockerfile b/Dockerfile
index 8ca8471..9ff5d32 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -38,4 +38,5 @@ WORKDIR ..
 
 RUN mkdir -p ~/catkin_ws/src
 ENV OsqpEigen_DIR=/osqp-eigen
+WORKDIR catkin_ws
 
diff --git a/rrt_star/__pycache__/env.cpython-37.pyc b/rrt_star/__pycache__/env.cpython-37.pyc
deleted file mode 100644
index 55b3d66..0000000
Binary files a/rrt_star/__pycache__/env.cpython-37.pyc and /dev/null differ
diff --git a/rrt_star/__pycache__/plotting.cpython-37.pyc b/rrt_star/__pycache__/plotting.cpython-37.pyc
deleted file mode 100644
index 86a7e0e..0000000
Binary files a/rrt_star/__pycache__/plotting.cpython-37.pyc and /dev/null differ
diff --git a/rrt_star/__pycache__/queue.cpython-37.pyc b/rrt_star/__pycache__/queue.cpython-37.pyc
deleted file mode 100644
index a33b622..0000000
Binary files a/rrt_star/__pycache__/queue.cpython-37.pyc and /dev/null differ
diff --git a/rrt_star/__pycache__/rrt_star.cpython-37.pyc b/rrt_star/__pycache__/rrt_star.cpython-37.pyc
deleted file mode 100644
index 6173384..0000000
Binary files a/rrt_star/__pycache__/rrt_star.cpython-37.pyc and /dev/null differ
diff --git a/rrt_star/__pycache__/utils.cpython-37.pyc b/rrt_star/__pycache__/utils.cpython-37.pyc
deleted file mode 100644
index 5b6515a..0000000
Binary files a/rrt_star/__pycache__/utils.cpython-37.pyc and /dev/null differ
diff --git a/rrt_star/rrt_star.py b/rrt_star/rrt_star.py
index 9eaedf4..d0940cb 100644
--- a/rrt_star/rrt_star.py
+++ b/rrt_star/rrt_star.py
@@ -85,8 +85,8 @@ def planning(self):
             node_new = self.new_state(node_near, node_rand)
 
             # print the sample number 
-            if k % 500 == 0:
-                print("Sample Number: ",k)
+            if k % 10 == 0:
+                print("Sample Number: ",k,"Number of vertices",len(self.list_of_vertices))
 
             # get a new node and if the ray connecting the new node with near node 
             # is collision free then we can add the vertex to our list of nodes
@@ -312,28 +312,22 @@ def cost(node_p):
     """
     @staticmethod
     def nearest_neighbor(node_list, n):
-        return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+        return node_list[int(np.argmin([np.linalg.norm([nd.x - n.x, nd.y - n.y])
                                         for nd in node_list]))]
 
 
-    
-   
-        
-
-
-
 
 def main():
     x_start = (18, 8)  # Starting node
     x_goal = (37, 18)
-    x_width = 50 
-    y_width = 30
+    x_width = 100 
+    y_width = 60
     #plotting.Plotting(x_start, x_goal,x_width,y_width).plot_grid("Test")
 
 
     # call order
     # x_start, x_goal, step length, search radius, number of samples
-    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 100,x_width,y_width)
+    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 200,x_width,y_width)
     rrt_star.planning()
     rrt_star.plot_final()
 
diff --git a/rrt_star/learning_rrt.py b/rrt_star_ros/learning_rrt.py
similarity index 74%
rename from rrt_star/learning_rrt.py
rename to rrt_star_ros/learning_rrt.py
index 7f03c8b..260708a 100644
--- a/rrt_star/learning_rrt.py
+++ b/rrt_star_ros/learning_rrt.py
@@ -18,9 +18,10 @@ def __init__(self,x=None,y=None,cost=0.0,parent_index=None):
 
 if __name__=="__main__":
     grid=np.load('porto_grid.npy')
-    #grid[2210:2270]
-    print(grid.shape)
-    plt.imshow(grid)
-    plt.xlim(2210,2857)
-    plt.ylim(2400,2700)
-    plt.show()
+    print(grid)
+    ##grid[2210:2270]
+    #print(grid.shape)
+    #plt.imshow(grid)
+    #plt.xlim(2210,2857)
+    #plt.ylim(2400,2700)
+    #plt.show()
diff --git a/rrt_star_ros/plotting.py b/rrt_star_ros/plotting.py
new file mode 100644
index 0000000..9dbcbb5
--- /dev/null
+++ b/rrt_star_ros/plotting.py
@@ -0,0 +1,131 @@
+""" Plotting Tools for Sampling-based algorithms
+@author: Patrick Musau
+
+Inspiried by Huming Zhou
+"""
+#import env
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import numpy as np
+import os
+import sys
+
+class Plotting:
+
+    """
+    x_start: 2D Point
+    y_start: 2D Point
+
+    free (0), 
+    occupied (100), 
+    and unknown (-1)
+    """
+
+
+    def __init__(self, grid_file,x_start,x_goal):
+
+        # create the figure
+        self.fig, self.ax = plt.subplots()
+
+        # the origin and res come from the map.yaml file
+        self.origin= (-100,-100)
+        self.res = 0.04
+        self.grid=np.load(grid_file).astype(int)
+        item = (np.where(self.grid>0))
+        item2 = (np.where(self.grid==0))
+        # just plot the boundaries
+        self.x_obs = (item[0] * self.res) + self.origin[0]
+        self.y_obs = (item[1] * self.res) + self.origin[1]
+        self.x_free = (item2[0] * self.res) + self.origin[0]
+        self.y_free = (item2[1] * self.res) + self.origin[1]
+    
+        
+        # get the start and goal
+        self.xI, self.xG = x_start, x_goal
+        # create object that defines the planning workspace
+    
+    def animation(self,nodelist,path,name, animation=False,block=False):
+        # plot the grid
+        self.ax.clear()
+        self.plot_grid(name)
+        # plot visited nodes
+        self.plot_visited(nodelist, animation)
+        # plot the final path 
+        self.plot_path(path,block=block)
+
+
+
+    """
+        I'm a visual learner so this method just shows me how points are sampled
+    """
+    def random_sample(self):
+        x = np.random.choice(self.x_free)
+        y = np.random.choice(self.y_free)
+        x_index = int(round((x - self.origin[0])/(self.res)))
+        y_index = int(round((y - self.origin[1])/(self.res)))
+        print(x,y,x_index,y_index)
+        print("Sanity Check")
+        print((x_index*self.res)+self.origin[0],(y_index*self.res)+self.origin[1],)
+        print(x_index,y_index,self.grid[x_index][y_index])
+        self.ax.plot([x], [y], "ks", linewidth=3,label="random_point")
+        plt.legend()
+        plt.show(block=True)
+
+
+    
+    # plot the 2d grid
+    def plot_grid(self, name,block=False):
+        
+
+
+        self.ax.plot(self.x_obs,self.y_obs,'k.',label="boundaries")
+        self.ax.plot(self.x_free,self.y_free,'y.',label="freespace")
+
+        self.ax.plot(self.xI[0], self.xI[1], "bs", linewidth=3,label="init")
+        self.ax.plot(self.xG[0], self.xG[1], "gs", linewidth=3,label="goal")
+
+        # set equal aspect ratio for figures
+        plt.xlabel("x(m)")
+        plt.ylabel("y(m)")
+        plt.title(name)
+        plt.axis("equal")
+        plt.legend()
+        plt.show(block=block)
+
+
+    # Go through the list of nodes and connect each node to it's parent 
+    # Pausing if you want to animate
+    #@staticmethod
+    def plot_visited(self,nodelist, animation):
+        if animation:
+            count = 0
+            for node in nodelist:
+                count += 1
+                if node.parent:
+                    self.ax.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+                    plt.gcf().canvas.mpl_connect('key_release_event',
+                                                 lambda event:
+                                                 [exit(0) if event.key == 'escape' else None])
+                    if count % 10 == 0:
+                        plt.pause(0.001)
+        else:
+            for node in nodelist:
+                if node.parent:
+                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+
+    # plotting a path via list comprehensions
+    #@staticmethod
+    def plot_path(self,path,block=False):
+        if len(path) != 0:
+            self.ax.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
+            plt.pause(0.01)
+        plt.show(block=block)
+
+
+if __name__ == "__main__":
+    x_start = (0,0)
+    x_goal = (1.422220,1.244794)
+    grid = 'porto_grid.npy'
+    pl = Plotting(grid,x_start,x_goal)
+    pl.plot_grid("Porto Occupancy Grid",block=False)
+    pl.random_sample()
diff --git a/rrt_star_ros/porto_grid.npy.zip b/rrt_star_ros/porto_grid.npy.zip
new file mode 100644
index 0000000..612a079
Binary files /dev/null and b/rrt_star_ros/porto_grid.npy.zip differ
diff --git a/rrt_star_ros/rrt_star.py b/rrt_star_ros/rrt_star.py
new file mode 100644
index 0000000..5f36e39
--- /dev/null
+++ b/rrt_star_ros/rrt_star.py
@@ -0,0 +1,361 @@
+import numpy as np 
+import math 
+
+import plotting
+
+# class def for tree nodes
+# It's up to you if you want to use this
+class Node(object):
+    def __init__(self,point,cost=0):
+        self.x = point[0]
+        self.y = point[1]
+        self.parent = None
+        self.cost = cost # only used in RRT*
+        self.is_root = False
+
+
+
+
+### RRT Star Class for occupancy grid
+# The inputs to the rrt start class are the goal, and start 
+# step_len:
+# goal_sample_rate
+# search_radius 
+# max number of iterations
+class RrtStar:
+    def __init__(self, x_start, x_goal, step_len,goal_sample_rate, search_radius, iter_max,grid):
+        # The goal and the start node are nodes within our empty graph
+        self.s_start = Node(x_start)
+        self.s_goal = Node(x_goal)
+
+        # Define the parameters for rrt
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_max = iter_max
+
+        # Initialize the list of vertices with the start point
+        self.list_of_vertices = [self.s_start]
+
+        # The path begins as an empty list
+        self.path = []
+
+        # create the plotting utility, this also has information about the obstcles 
+        # and free space
+        self.plotting = plotting.Plotting(grid,x_start,x_goal)
+
+
+        # get the obstacle boundaries and the free space
+        self.x_obs = self.plotting.x_obs
+        self.y_obs = self.plotting.y_obs
+        self.x_free = self.plotting.x_free
+        self.y_free = self.plotting.y_free
+
+        # get the origin and resolution of the occupancy grid
+        self.origin= self.plotting.origin
+        self.res = self.plotting.res
+    
+
+        # This is the occupancy map 
+        self.grid = self.plotting.grid
+
+    def planning(self):
+        # Iter-Max looks the number of samples described in sertac karaman's paper
+        for k in range(self.iter_max):
+            
+            # need to generate a random sample in the workspace
+            node_rand = self.generate_random_node(self.goal_sample_rate)
+
+            # find the nearest neighbor in the tree (probably euclidean)
+            node_near = self.nearest_neighbor(self.list_of_vertices, node_rand)
+
+            # This is like a steering function, we take a step in the direction and
+            # angle of a newly sampled point (so for f1tenth it might be using the ackermann dynamics)
+            # who knows 
+            node_new = self.new_state(node_near, node_rand)
+
+            # print the sample number 
+            if k % 10 == 0:
+                print("Sample Number: ",k,"Number of vertices",len(self.list_of_vertices))
+
+            # get a new node and if the ray connecting the new node with near node 
+            # is collision free then we can add the vertex to our list of nodes
+            if node_new and not self.is_collision(node_near, node_new):
+
+                # find the closest neighbors this is a list
+                neighbor_indices = self.find_nearest_neighbors(node_new)
+
+                # append the new node
+                self.list_of_vertices.append(node_new)
+                
+                # Go through the list of neighbors
+                if neighbor_indices:
+                    # choose the parent node from the list of neighbors, the parent will be the node
+                    # with the lowest cost
+                    self.choose_parent(node_new, neighbor_indices)
+
+                    # rewire the tree, this ensure's the optimality of the search
+                    self.rewire(node_new, neighbor_indices)
+            
+
+            # get the index of the minimum cost vertex within a step length of the goal 
+            index = self.search_goal_parent()
+            self.path = self.extract_path(self.list_of_vertices[index])
+
+            # Plotting 
+            self.plotting.animation(self.list_of_vertices, self.path, "rrt*, N = " + str(self.iter_max),True)
+
+
+
+
+
+    """ starting from the vertex that has been identified as closest the goal work backwards.
+    """
+    def extract_path(self, node_end):
+        
+        # last node in the path is the goal node
+        path = [[self.s_goal.x, self.s_goal.y]]
+        node = node_end
+        
+        # from this node ask for parents
+        while node.parent is not None:
+            path.append([node.x, node.y])
+            node = node.parent
+        path.append([node.x, node.y])
+
+        return path
+
+
+    """function that checks for collisions in the newly sampled point"""
+    def is_collision(self,start_node,end_node):
+
+
+        # get the steps in the right direction
+        x_len = end_node.x - start_node.x 
+        y_len = end_node.y - start_node.y 
+
+        # discretize the path and check if any of the grid points are occupied 
+        for i in range(0,101):
+            
+            x = start_node.x + (0.01) * x_len
+            y = start_node.y + (0.01) * y_len
+            x_index = int(round((x - self.origin[0])/(self.res)))
+            y_index = int(round((y - self.origin[1])/(self.res)))
+
+            # in the occupancy grid a value of 100 means occupied, -1 is unknown
+            # better to assume unknown is collision
+            if (self.grid[x_index][y_index]==100 or self.grid[x_index][y_index]==-1):
+                return True
+        return False
+            
+
+
+    """
+        Return the node with the minimum cost (in this case it's a euclidean distance based)
+        cost, this function doesn't return anything other than connect the graph
+    """
+    def choose_parent(self, node_new, neighbor_indices):
+        cost = [self.get_new_cost(self.list_of_vertices[i], node_new) for i in neighbor_indices]
+
+        # get the parent with the minimum cost
+        cost_min_index = neighbor_indices[int(np.argmin(cost))]
+
+        # the new node's parent is the node with the minimum cost
+        node_new.parent = self.list_of_vertices[cost_min_index]
+
+
+    """
+        check if path is feasible
+    """
+    def path_found(self):
+        if self.s_goal in self.path and self.s_start in self.path:
+            print("Path Found")
+        else:
+            print("Still Searching")
+        
+
+    
+    """
+        This is another function that can benefit from a vectorized implementation
+        It computes the distance to each vertex within the tree and either returns the vertex
+        that is within a step radius of the goal and has a minimum cost or the index of the 
+        newest node.
+
+    """
+    def search_goal_parent(self):
+
+        # create a list of distances to the goal
+        dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.list_of_vertices]
+
+        # get all the nodes that are within a step length of the goal
+        # return all the indices that satisify the criteria
+        node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len]
+        
+        # with this list of nodes that are within a step length of the goal, compute the distances to each of the nodes,
+        # and also ensure that connecting this node with the goal doesn't result in a collision 
+        if len(node_index) > 0:
+            cost_list = [dist_list[i] + self.cost(self.list_of_vertices[i]) for i in node_index
+                         if not self.is_collision(self.list_of_vertices[i], self.s_goal)]
+            if(len(cost_list)>0):
+                return node_index[int(np.argmin(cost_list))]
+        
+
+        ## TBD: need to grok this a little more
+        return len(self.list_of_vertices) - 1
+
+
+
+    """
+        function that generates a new node based on a random sample
+
+        Node start is the nearest node in the tree
+        Node goal is the random node that we just randomly sampled
+
+    """
+    def new_state(self, node_start, node_goal):
+
+        # get the distance between these two and the angle
+
+        dist, theta = self.get_distance_and_angle(node_start, node_goal)
+
+        # instead of using this new point we take a step in that direction with a max distance 
+        # step_len
+        dist = min(self.step_len, dist)
+
+        # create the new node
+        node_new = Node((node_start.x + dist * math.cos(theta),
+                         node_start.y + dist * math.sin(theta)))
+        
+        # make the parent of this node the nearest node
+        node_new.parent = node_start
+
+        return node_new
+
+
+    """
+        Rewire the tree. Performing this operation is what gives rrt_star asymptotic optimality. Basically if adding this node to it's neighbors 
+        decreases the cost of those paths then we should do so by rewiring the tree
+    """
+    def rewire(self, node_new, neighbor_indices):
+
+        # iterate through the list of neighbor nodes
+        for i in neighbor_indices:
+            node_neighbor = self.list_of_vertices[i]
+            
+            # if the cost at the current vertex is greater when adding the new node
+            # then we should rewire the tree by making this new node it's parent, thereby decreasing this path's length
+            if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor):
+                node_neighbor.parent = node_new
+
+
+
+    """
+    This function returns the nearest neighbors of new node. The search radius is the parameter that ensures 
+    asymptotic optimality, it has a minimum distance between the step len and this other thing that I'm still wondering
+    about tbh. This function can be sped up (vectorized implementation)
+
+    """
+    def find_nearest_neighbors(self, node_new):
+
+        
+        # n is the number of vertices
+        n = len(self.list_of_vertices) + 1
+        
+        # this is the optimality criterion
+        r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
+
+        # yeah this really should be vectorized compute the distance to every node from this one
+        # this is an ordered list of each vertex which is a node
+        dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.list_of_vertices]
+
+        # return only those nodes that are within the search radius and that don't result in an intersection with 
+        # an obstacle
+        dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and
+                            not self.is_collision(node_new, self.list_of_vertices[ind])]
+
+        # return the list of obstacles
+
+        return dist_table_index
+
+
+    """
+        This method should randomly sample the free space, and returns a viable point
+
+        Args:
+        Returns:
+            (x, y) (float float): a tuple representing the sampled point
+
+        """
+    def generate_random_node(self,goal_sample_rate):
+
+        if np.random.random() > goal_sample_rate:
+            x = np.random.choice(self.x_free)
+            y = np.random.choice(self.y_free)
+            return Node((x,y))
+        return self.s_goal
+
+
+    """
+        Function that returns new between two nodes, the cost of the starting node and the end node
+    """
+    def get_new_cost(self, node_start, node_end):
+        dist, _ = self.get_distance_and_angle(node_start, node_end)
+
+        return self.cost(node_start) + dist
+
+    """
+        nearest node in terms of euclidean distance
+    """
+    @staticmethod
+    def nearest_neighbor(node_list, n):
+        return node_list[int(np.argmin([np.linalg.norm([nd.x - n.x, nd.y - n.y])
+                                        for nd in node_list]))]
+
+    """
+        Function that returns the euclidean distance and angle and angle betwen two nodes
+    """
+    @staticmethod
+    def get_distance_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+
+    """
+        computes the euclidean distance of the path of a node
+        This can be sped up by storing the cost at each node
+
+    """
+    @staticmethod
+    def cost(node_p):
+        node = node_p
+        cost = 0.0
+
+        while node.parent:
+            cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+            node = node.parent
+
+        return cost
+
+    
+    """
+        Function that plots final solution obtained
+    """
+    def plot_final(self):
+        self.plotting.animation(self.list_of_vertices, self.path, "Porto Final Solution, N={}".format(self.iter_max),True,True)
+
+
+
+    
+if __name__ == "__main__":
+    x_start = (0,0)
+    x_goal = (1.422220,1.244794)
+    grid = 'porto_grid.npy'
+    step_length = 0.10 
+    goal_sample_rate = 0.10
+    search_radius = 1.00
+    n_samples = 100
+
+    rrt_star = RrtStar(x_start, x_goal, step_length,goal_sample_rate, search_radius, n_samples,grid)
+    rrt_star.planning()
+    rrt_star.plot_final()
diff --git a/rrt_star_ros/utils.py b/rrt_star_ros/utils.py
new file mode 100644
index 0000000..95a7a4d
--- /dev/null
+++ b/rrt_star_ros/utils.py
@@ -0,0 +1,20 @@
+""" 
+Utilities for collision checking in rrt_star
+@author: patrick musau
+Inspired by huiming zhou and Hongrui Zheng
+
+Overall forom this paper: https://arxiv.org/pdf/1105.1186.pdf
+"""
+
+
+
+import math
+import numpy as np
+import os
+import sys
+
+# I need this file to get the occupancy grid
+import plotting
+
+# import node from rrt_star (x,y,parent)
+from rrt_star import Node
\ No newline at end of file