diff --git a/Dockerfile b/Dockerfile
index fc2cbb5..69d97e3 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -26,7 +26,7 @@ WORKDIR /catkin_ws/src
 COPY src .
 COPY entrypoint.sh .
 RUN touch ./suas_sim/CATKIN_IGNORE \
-    && /bin/bash -c 'source /opt/ros/melodic/setup.sh && cd /catkin_ws && catkin_make' \
+    && /bin/bash -c 'source /opt/ros/melodic/setup.sh && cd /catkin_ws && catkin_make && catkin_make run_tests' \
     && chown -R $USERNAME:$USER_GID /catkin_ws \
     && echo "source /catkin_ws/devel/setup.sh" > /home/${USERNAME}/.bashrc \
     && chmod 0755 entrypoint.sh
@@ -35,4 +35,4 @@ ENV DEBIAN_FRONTEND dialog
 USER $USERNAME
 
 ENTRYPOINT ["/catkin_ws/src/entrypoint.sh"]
-CMD bash
\ No newline at end of file
+CMD bash
diff --git a/build.sh b/build.sh
new file mode 100755
index 0000000..c5df5de
--- /dev/null
+++ b/build.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+docker build -t suas .
diff --git a/src/suas_planning/CMakeLists.txt b/src/suas_planning/CMakeLists.txt
index 7dec70c..29550e9 100644
--- a/src/suas_planning/CMakeLists.txt
+++ b/src/suas_planning/CMakeLists.txt
@@ -193,19 +193,6 @@ include_directories(
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
 
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_suas_planning.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
 set(NODE_SOURCES
   src/Obstacle.cpp
   src/CircularObstacle.cpp
@@ -215,4 +202,22 @@ set(NODE_SOURCES
   src/GlobalWaypointPlanner.cpp)
 
 add_executable(global_planner_node ${NODE_SOURCES})
-target_link_libraries(global_planner_node ${catkin_LIBRARIES})
\ No newline at end of file
+target_link_libraries(global_planner_node ${catkin_LIBRARIES})
+
+#############
+## Testing ##
+#############
+
+# statically link
+add_library(global_planner_node_lib ${NODE_SOURCES})
+target_link_libraries(global_planner_node_lib ${catkin_LIBRARIES})
+
+## Add gtest based cpp test target and link libraries
+catkin_add_gtest(${PROJECT_NAME}-test test/test_suas_planning.cpp)
+if(TARGET ${PROJECT_NAME}-test)
+   target_link_libraries(${PROJECT_NAME}-test global_planner_node_lib)
+endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
diff --git a/src/suas_planning/test/test_suas_planning.cpp b/src/suas_planning/test/test_suas_planning.cpp
new file mode 100644
index 0000000..a65c569
--- /dev/null
+++ b/src/suas_planning/test/test_suas_planning.cpp
@@ -0,0 +1,117 @@
+#include "../include/PlanningPoints.hpp"
+#include <gtest/gtest.h>
+#include <vector>
+#include <cstdint>
+#include <memory>
+#include <iostream>
+
+#define MAP_DIM 100
+
+class CircularObstacleTest : public testing::Test
+{
+protected:
+    void SetUp() override
+    {
+        x = 32;
+        y = 32;
+        radius = 12;
+        map.resize(MAP_DIM * MAP_DIM);
+        test_obst = suas_planning::CircularObstacle((double)x, (double)y, (double)radius);
+        mmi = std::make_unique<suas_planning::MapMetaInfo>(map, MAP_DIM, MAP_DIM);
+    }
+
+    void TearDown() override
+    {
+
+    }
+
+    // members
+    std::vector<int8_t> map;
+    std::unique_ptr<suas_planning::MapMetaInfo> mmi;
+    suas_planning::CircularObstacle test_obst;
+    int x;
+    int y;
+    int radius;
+    int reference[25*25] = {
+        0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,
+        0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,
+        0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,
+        0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,
+        0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,
+        0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,
+        0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,
+        0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,
+        0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,
+        1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
+        1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
+        1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
+        1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,
+        1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
+        1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
+        1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
+        0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,
+        0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,
+        0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,
+        0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,
+        0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,
+        0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,
+        0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,
+        0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,
+        0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0
+    };
+};
+
+TEST_F(CircularObstacleTest, TestGetLinearIndex)
+{
+    int asserted_index = y * MAP_DIM + x;
+    ASSERT_EQ(asserted_index, suas_planning::Obstacle::GetLinearIndex(y, x, MAP_DIM));
+    ASSERT_EQ(MAP_DIM, suas_planning::Obstacle::GetLinearIndex(1, 0, MAP_DIM));
+    ASSERT_EQ(0, suas_planning::Obstacle::GetLinearIndex(0, 0, MAP_DIM));
+}
+
+TEST_F(CircularObstacleTest, TestCheckBounds)
+{
+    int x_min = x - radius;
+    int x_max = x + radius;
+    int y_min = y - radius;
+    int y_max = y + radius;
+
+    ASSERT_EQ(x_min, test_obst.GetMinX());
+    ASSERT_EQ(x_max, test_obst.GetMaxX());
+    ASSERT_EQ(y_min, test_obst.GetMinY());
+    ASSERT_EQ(y_max, test_obst.GetMaxY());
+}
+
+TEST_F(CircularObstacleTest, TestPosition)
+{
+    test_obst.PlotObstacle(map, *mmi);
+    int index = suas_planning::Obstacle::GetLinearIndex(y, x, MAP_DIM);
+    ASSERT_EQ(map[index], 100);
+}
+
+TEST_F(CircularObstacleTest, TestRasterize)
+{
+    test_obst.PlotObstacle(map, *mmi);
+    int start_x = x - radius;
+    int start_y = y - radius;
+    int index = suas_planning::Obstacle::GetLinearIndex(y, x, MAP_DIM);
+
+    int ri = 0;
+    for (int i = 0; i < radius * 2 + 1; i++)
+    {
+        for (int j = 0; j < radius * 2 + 1; j++)
+        {
+            int v = (int)map[suas_planning::Obstacle::GetLinearIndex(start_y + i, start_x + j, MAP_DIM)] / 100;
+            ASSERT_EQ(v, reference[ri]);
+            //std::cout << v << " ";
+            ri++;
+        }
+        //std::cout << std::endl;
+    }
+}
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}