diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0cbc99c
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,72 @@
+# Created by https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++
+# Edit at https://www.toptal.com/developers/gitignore?templates=cmake,visualstudiocode,c++
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### CMake ###
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+
+### CMake Patch ###
+# External projects
+*-prefix/
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+*.code-workspace
+
+# Local History for Visual Studio Code
+.history/
+
+### VisualStudioCode Patch ###
+# Ignore all local history of files
+.history
+.ionide
+
+# End of https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++
+.vscode/settings.json
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8842b0b..38ce075 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,7 +35,7 @@ catkin_package(
 add_library(${PROJECT_NAME}
         src/common.cpp
         src/${PROJECT_NAME}.cpp
-        src/spiral_stc.cpp
+        src/boustrophedon_stc.cpp
         )
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(${PROJECT_NAME}
@@ -65,13 +65,13 @@ catkin_install_python(
 if (CATKIN_ENABLE_TESTING)
     catkin_add_gtest(test_common test/src/test_common.cpp test/src/util.cpp src/common.cpp)
 
-    catkin_add_gtest(test_spiral_stc test/src/test_spiral_stc.cpp test/src/util.cpp src/spiral_stc.cpp src/common.cpp src/${PROJECT_NAME}.cpp)
-    add_dependencies(test_spiral_stc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-    target_link_libraries(test_spiral_stc ${catkin_LIBRARIES})
+    catkin_add_gtest(test_boustrophedon_stc test/src/test_boustrophedon_stc.cpp test/src/util.cpp src/boustrophedon_stc.cpp src/common.cpp src/${PROJECT_NAME}.cpp)
+    add_dependencies(test_boustrophedon_stc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+    target_link_libraries(test_boustrophedon_stc ${catkin_LIBRARIES})
 
     find_package(OpenCV)
     include_directories(${OpenCV_INCLUDE_DIRS})
-    target_link_libraries(test_spiral_stc ${OpenCV_LIBRARIES})
+    target_link_libraries(test_boustrophedon_stc ${OpenCV_LIBRARIES})
 
     add_rostest(test/${PROJECT_NAME}/test_${PROJECT_NAME}.test)
 
diff --git a/README.md b/README.md
index a4b23ea..c0c9e56 100644
--- a/README.md
+++ b/README.md
@@ -1,12 +1,19 @@
-# Full Coverage Path Planner (FCPP)
+# Boustrophedon Full Coverage Path Planner (Modified)
 
+## Acknowledgement
+
+This package is a modification of the Full Coverage Path Planner package from Nobleo.
+
+It is modified such that a Boustrophedon Pattern is used to plan the path rather than a Spiral algorithm in the original package.
+
+Refer to the original package here: http://wiki.ros.org/full_coverage_path_planner
 ## Overview
 
-This package provides an implementation of a Full Coverage Path Planner (FCPP) using the Backtracking Spiral Algorithm (BSA), see [1].
+This package provides an implementation of a Full Coverage Path Planner (FCPP) using the Boustrophedon Pattern. see [1] and [2].
 
 This packages acts as a global planner plugin to the Move Base package (http://wiki.ros.org/move_base).
 
-![BSA](doc/fcpp_robot_0_5m_plus_tool_0_2m.png)
+![BSA](doc/fcpp_modified_boustrophedon.png)
 
 The user can configure robot radius and tool radius separately:
 
@@ -15,17 +22,19 @@ The user can configure robot radius and tool radius separately:
 
 **Keywords:** coverage path planning, move base
 
-### License
+### Authors
 
 Apache 2.0
 
+**Package modified by Ethan Kim, ethanc.kim@uwaterloo.ca, MapaRobo Inc.**
+
 **Author(s): Yury Brodskiy, Ferry Schoenmakers, Tim Clephas, Jerrel Unkel, Loy van Beek, Cesar lopez**
 
 **Maintainer:  Cesar Lopez, cesar.lopez@nobleo.nl**
 
 **Affiliation: Nobleo Projects BV, Eindhoven, the Netherlands**
 
-The Full Coverage Path Planner package has been tested under [ROS] Melodic and Ubuntu 18.04.
+The Modified package has been tested under [ROS] Noetic and Ubuntu 20.04.
 
 ## Installation
 
@@ -57,8 +66,8 @@ All tests can be run using:
 #### test_common
 Unit test that checks the basic functions used by the repository
 
-#### test_spiral_stc
-Unit test that checks the basis spiral algorithm for full coverage. The test is performed for different situations to check that the algorithm coverage the accessible map cells. A test is also performed in randomly generated maps.
+#### test_boustrophedon_stc
+Unit test that checks the basis boustrophedon algorithm for full coverage. The test is performed for different situations to check that the algorithm coverage the accessible map cells. A test is also performed in randomly generated maps.
 
 #### test_full_coverage_path_planner.test
 ROS system test that checks the full coverage path planner together with a tracking pid. A simulation is run such that a robot moves to fully cover the accessible cells in a given map.
@@ -131,8 +140,8 @@ The CoverageProgressNode keeps track of coverage progress. It does this by perio
 
 
 ## Plugins
-### full_coverage_path_planner/SpiralSTC
-For use in move_base(\_flex) as "base_global_planner"="full_coverage_path_planner/SpiralSTC". It uses global_cost_map and global_costmap/robot_radius.
+### full_coverage_path_planner/BoustrophedonSTC
+For use in move_base(\_flex) as "base_global_planner"="full_coverage_path_planner/BoustrophedonSTC". It uses global_cost_map and global_costmap/robot_radius.
 
 #### Parameters
 
@@ -142,18 +151,10 @@ For use in move_base(\_flex) as "base_global_planner"="full_coverage_path_planne
 
 ## References
 
-[1] GONZALEZ, Enrique, et al. BSA: A complete coverage algorithm. In: Proceedings of the 2005 IEEE International Conference on Robotics and Automation. IEEE, 2005. p. 2040-2044.
-
-## Bugs & Feature Requests
-
-Please report bugs and request features using the [Issue Tracker](https://github.com/nobleo/full_coverage_path_planner/issues).
-
-
-[ROS]: http://www.ros.org
-[rviz]: http://wiki.ros.org/rviz
-[MBF]: http://wiki.ros.org/move_base_flex
+[1] Choset, Howie, and Philippe Pignon. "Coverage path planning: The boustrophedon cellular decomposition." Field and service robotics. Springer, London, 1998.
+[2] Zelinsky, Alexander, et al. "Planning paths of complete coverage of an unstructured environment by a mobile robot." Proceedings of international conference on advanced robotics. Vol. 13. 1993.
 
-## Acknowledgments
+## Acknowledgments from Original Authors
 
 <!--
     ROSIN acknowledgement from the ROSIN press kit
diff --git a/doc/fcpp_modified_boustrophedon.png b/doc/fcpp_modified_boustrophedon.png
new file mode 100644
index 0000000..1d053bf
Binary files /dev/null and b/doc/fcpp_modified_boustrophedon.png differ
diff --git a/fcpp_plugin.xml b/fcpp_plugin.xml
index 9411201..5f9522a 100644
--- a/fcpp_plugin.xml
+++ b/fcpp_plugin.xml
@@ -1,9 +1,9 @@
 <library path="lib/libfull_coverage_path_planner">
-  <class name="full_coverage_path_planner/SpiralSTC" type="full_coverage_path_planner::SpiralSTC" base_class_type="nav_core::BaseGlobalPlanner">
+  <class name="full_coverage_path_planner/BoustrophedonSTC" type="full_coverage_path_planner::BoustrophedonSTC" base_class_type="nav_core::BaseGlobalPlanner">
     <description>
-      Plans a path that covers all accessible points in a costmap by using Spiral-STC.
-      Spiral-STC works by following the walls and spiraling inwards until it cannot go further.
-      Then it uses A* to go back outside of the current spiral and then spirals again.
+      Plans a path that covers all accessible points in a costmap by using Boustrophedon-STC (Spanning Tree Coverage).
+      In essence, the robot moves forward until an obstacle or visited node is met, then turns right or left (making a boustrophedon pattern)
+      When stuck while completing a boustrophedon pattern, use A* to get out again and start a new boustrophedon, until A* can't find a path to uncovered cells
     </description>
   </class>
 </library>
diff --git a/include/full_coverage_path_planner/spiral_stc.h b/include/full_coverage_path_planner/boustrophedon_stc.h
similarity index 64%
rename from include/full_coverage_path_planner/spiral_stc.h
rename to include/full_coverage_path_planner/boustrophedon_stc.h
index 1d843b2..87ac82b 100644
--- a/include/full_coverage_path_planner/spiral_stc.h
+++ b/include/full_coverage_path_planner/boustrophedon_stc.h
@@ -20,34 +20,34 @@
 
 using std::string;
 
-#ifndef FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H
-#define FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H
+#ifndef FULL_COVERAGE_PATH_PLANNER_BOUSTROPHEDON_STC_H
+#define FULL_COVERAGE_PATH_PLANNER_BOUSTROPHEDON_STC_H
 
 #include "full_coverage_path_planner/full_coverage_path_planner.h"
 namespace full_coverage_path_planner
 {
-class SpiralSTC : public nav_core::BaseGlobalPlanner, private full_coverage_path_planner::FullCoveragePathPlanner
+class BoustrophedonSTC : public nav_core::BaseGlobalPlanner, private full_coverage_path_planner::FullCoveragePathPlanner
 {
 public:
   /**
-   * Find a path that spirals inwards from init until an obstacle is seen in the grid
+   * Find a path that does the boustrophedon pattern starting from init until a dead end is reached in the grid
    * @param grid 2D grid of bools. true == occupied/blocked/obstacle
    * @param init start position
-   * @param visited all the nodes visited by the spiral
-   * @return list of nodes that form the spiral
+   * @param visited all the nodes visited by the boustrophedon pattern
+   * @return list of nodes that form the boustrophedon pattern
    */
-  static std::list<gridNode_t> spiral(std::vector<std::vector<bool> > const &grid, std::list<gridNode_t> &init,
+  static std::list<gridNode_t> boustrophedon(std::vector<std::vector<bool> > const &grid, std::list<gridNode_t> &init,
                                       std::vector<std::vector<bool> > &visited);
 
   /**
-   * Perform Spiral-STC (Spanning Tree Coverage) coverage path planning.
-   * In essence, the robot moves forward until an obstacle or visited node is met, then turns right (making a spiral)
-   * When stuck in the middle of the spiral, use A* to get out again and start a new spiral, until a* can't find a path to uncovered cells
+   * Perform Boustrophedon-STC (Spanning Tree Coverage) coverage path planning.
+   * In essence, the robot moves forward until an obstacle or visited node is met, then turns right or left (making a boustrophedon pattern)
+   * When stuck in the middle of the boustrophedon, use A* to get out again and start a new boustrophedon, until a* can't find a path to uncovered cells
    * @param grid
    * @param init
    * @return
    */
-  static std::list<Point_t> spiral_stc(std::vector<std::vector<bool> > const &grid,
+  static std::list<Point_t> boustrophedon_stc(std::vector<std::vector<bool> > const &grid,
                                         Point_t &init,
                                         int &multiple_pass_counter,
                                         int &visited_counter);
@@ -72,4 +72,4 @@ class SpiralSTC : public nav_core::BaseGlobalPlanner, private full_coverage_path
 };
 
 }  // namespace full_coverage_path_planner
-#endif  // FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H
+#endif  // FULL_COVERAGE_PATH_PLANNER_BOUSTROPHEDON_STC_H
diff --git a/include/full_coverage_path_planner/common.h b/include/full_coverage_path_planner/common.h
index 7d46c21..376dc9f 100644
--- a/include/full_coverage_path_planner/common.h
+++ b/include/full_coverage_path_planner/common.h
@@ -61,6 +61,15 @@ enum
   eNodeVisited = true
 };
 
+enum
+{
+  point = 0,
+  east = 1,
+  west = 2,
+  north = 3,
+  south = 4
+};
+
 /**
  * Find the distance from poi to the closest point in goals
  * @param poi Starting point
@@ -124,4 +133,39 @@ void printGrid(std::vector<std::vector<bool> > const& grid);
  * @return a list of points that have the given value_to_search
  */
 std::list<Point_t> map_2_goals(std::vector<std::vector<bool> > const& grid, bool value_to_search);
+
+/**
+ * Prints pathNodes in the terminal
+ * @param pathNodes pathNodes to be printed in the terminal
+ */
+void printPathNodes(std::list<gridNode_t> pathNodes);
+
+/**
+ * returns true only if the desired move is valid
+ * @param x2 x coordinate of desired position
+ * @param y2 y coordinate of desired position
+ * @param nCols
+ * @param nRows 
+ * @param grid internal map representation - 2D grid of bools. true == occupied/blocked/obstacle
+ * @param visited 2D grid of bools. true == visited
+ */
+bool validMove(int x2, int y2, int nCols, int nRows,
+               std::vector<std::vector<bool> > const& grid,
+               std::vector<std::vector<bool> > const& visited);
+
+/**
+ * Adds node in (x2, y2) into the list of pathNodes, and marks the node as visited
+ */
+void addNodeToList(int x2, int y2, std::list<gridNode_t>& pathNodes,
+                   std::vector<std::vector<bool>>& visited);
+
+/**
+ * Returns direction in which most free space is visible when given the robot's current location
+ * @param ignoreDir ignores a single direction specified. Pass 0 (point) to consider all four directions.
+ */
+int dirWithMostSpace(int x2, int y2, int nCols, int nRows,
+                     std::vector<std::vector<bool> > const& grid,
+                     std::vector<std::vector<bool> > const& visited,
+                     int ignoreDir);
+
 #endif  // FULL_COVERAGE_PATH_PLANNER_COMMON_H
diff --git a/include/full_coverage_path_planner/full_coverage_path_planner.h b/include/full_coverage_path_planner/full_coverage_path_planner.h
index 763d098..9a2f1ce 100644
--- a/include/full_coverage_path_planner/full_coverage_path_planner.h
+++ b/include/full_coverage_path_planner/full_coverage_path_planner.h
@@ -79,12 +79,28 @@ class FullCoveragePathPlanner
   /**
    * Convert internal representation of a to a ROS path
    * @param start Start pose of robot
-   * @param goalpoints Goal points from Spiral Algorithm
+   * @param goalpoints Goal points from Boustrophedon Algorithm
    * @param plan  Output plan variable
    */
   void parsePointlist2Plan(const geometry_msgs::PoseStamped& start, std::list<Point_t> const& goalpoints,
                            std::vector<geometry_msgs::PoseStamped>& plan);
 
+  /**
+   * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile
+   * @param costmap_grid_ Costmap representation. Cells higher that 65 are considered occupied
+   * @param grid internal map representation
+   * @param tileSize size (in meters) of a cell. This can be the robot's size
+   * @param realStart Start position of the robot (in meters)
+   * @param scaledStart Start position of the robot on the grid
+   * @return success
+   */
+  bool parseCostmap(costmap_2d::Costmap2D* costmap_grid_,
+                    std::vector<std::vector<bool> >& grid,
+                    float robotRadius,
+                    float toolRadius,
+                    geometry_msgs::PoseStamped const& realStart,
+                    Point_t& scaledStart);
+
   /**
    * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile
    * @param cpp_grid_ ROS occupancy grid representation. Cells higher that 65 are considered occupied
@@ -103,6 +119,9 @@ class FullCoveragePathPlanner
   ros::Publisher plan_pub_;
   ros::ServiceClient cpp_grid_client_;
   nav_msgs::OccupancyGrid cpp_grid_;
+  // Using costmap instead of Occupancy Grid from map server as the costmap updates periodically.
+  costmap_2d::Costmap2DROS* costmap_ros_;
+  costmap_2d::Costmap2D* costmap_;
   float robot_radius_;
   float tool_radius_;
   float plan_resolution_;
@@ -111,14 +130,14 @@ class FullCoveragePathPlanner
   bool initialized_;
   geometry_msgs::PoseStamped previous_goal_;
 
-  struct spiral_cpp_metrics_type
+  struct boustrophedon_cpp_metrics_type
   {
     int visited_counter;
     int multiple_pass_counter;
     int accessible_counter;
     double total_area_covered;
   };
-  spiral_cpp_metrics_type spiral_cpp_metrics_;
+  boustrophedon_cpp_metrics_type boustrophedon_cpp_metrics_;
 };
 
 
diff --git a/maps/square.png b/maps/square.png
new file mode 100644
index 0000000..e747bdf
Binary files /dev/null and b/maps/square.png differ
diff --git a/maps/square.yaml b/maps/square.yaml
new file mode 100644
index 0000000..30b54e8
--- /dev/null
+++ b/maps/square.yaml
@@ -0,0 +1,6 @@
+image: square.png
+resolution: 0.05
+origin: [-5, -5, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/package.xml b/package.xml
index f03585e..3c5cbc6 100644
--- a/package.xml
+++ b/package.xml
@@ -1,14 +1,18 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>full_coverage_path_planner</name>
-  <version>0.6.4</version>
-  <description>Full coverage path planning provides a move_base_flex plugin that can plan a path that will fully cover a given area</description>
+  <version>0.6.5</version>
+  <description>
+    Full coverage path planning provides a move_base_flex plugin that can plan a path that will fully cover a given area.
+    This package has been modified such that a boustophedon pattern is used rather than a spiral algorithm.
+  </description>
   <author email="cesar.lopez@nobleo.nl">Cesar Lopez</author>
   <author email="ferry.schoenmakers@nobleo.nl">Ferry Schoenmakers</author>
   <author email="tim.clephas@nobleo.nl">Tim Clephas</author>
   <author email="jerrel.unkel@nobleo.nl">Jerrel Unkel</author>
   <author>Loy van Beek</author>
   <author>Yury Brodskiy</author>
+  <author email="ethanc.kim@uwaterloo.ca">Ethan Kim</author>
   <maintainer email="cesar.lopez@nobleo.nl">Cesar Lopez</maintainer>
 
   <license>Apache 2.0</license>
diff --git a/src/boustrophedon_stc.cpp b/src/boustrophedon_stc.cpp
new file mode 100644
index 0000000..d6fbe24
--- /dev/null
+++ b/src/boustrophedon_stc.cpp
@@ -0,0 +1,362 @@
+//
+// Copyright [2020] Nobleo Technology"  [legal/copyright]
+//
+#include <algorithm>
+#include <iostream>
+#include <list>
+#include <string>
+#include <vector>
+
+#include "full_coverage_path_planner/boustrophedon_stc.h"
+#include <pluginlib/class_list_macros.h>
+
+// register this planner as a BaseGlobalPlanner plugin
+PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::BoustrophedonSTC, nav_core::BaseGlobalPlanner)
+
+int pattern_dir_ = point;
+
+namespace full_coverage_path_planner
+{
+void BoustrophedonSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
+{
+  if (!initialized_)
+  {
+    // Create a publisher to visualize the plan
+    ros::NodeHandle private_nh("~/");
+    ros::NodeHandle nh, private_named_nh("~/" + name);
+
+    plan_pub_ = private_named_nh.advertise<nav_msgs::Path>("plan", 1);
+    // Try to request the cpp-grid from the cpp_grid map_server
+    cpp_grid_client_ = nh.serviceClient<nav_msgs::GetMap>("static_map");
+    // Get the cost map:
+    costmap_ros_ = costmap_ros;
+    costmap_ = costmap_ros->getCostmap();
+
+    // Define  robot radius (radius) parameter
+    float robot_radius_default = 0.5f;
+    private_named_nh.param<float>("robot_radius", robot_radius_, robot_radius_default);
+    // Define  tool radius (radius) parameter
+    float tool_radius_default = 0.5f;
+    private_named_nh.param<float>("tool_radius", tool_radius_, tool_radius_default);
+    initialized_ = true;
+  }
+}
+
+std::list<gridNode_t> BoustrophedonSTC::boustrophedon(std::vector<std::vector<bool> > const& grid, std::list<gridNode_t>& init,
+                                        std::vector<std::vector<bool> >& visited)
+{
+  int dx, dy, x2, y2, i, nRows = grid.size(), nCols = grid[0].size();
+  // Mountain pattern filling of the open space
+  // Copy incoming list to 'end'
+  std::list<gridNode_t> pathNodes(init);
+
+  // Set starting pos
+  x2 = pathNodes.back().pos.x;
+  y2 = pathNodes.back().pos.y; 
+  // set initial direction based on space visible from initial pos
+  int robot_dir = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, point);
+  // set dx and dy based on robot_dir
+  switch(robot_dir) {
+    case east: // 1
+      dx = +1;
+      dy = 0;
+      break;
+    case west: // 2
+      dx = -1;
+      dy = 0;
+      break;
+    case north: // 3
+      dx = 0;
+      dy = +1;
+      break;
+    case south: // 4
+      dx = 0;
+      dy = -1;
+      break;
+    default:
+      ROS_ERROR("Full Coverage Path Planner: NO INITIAL ROBOT DIRECTION CALCULATED. This is a logic error that must be fixed by editing boustrophedon_stc.cpp. Will travel east for now.");
+      robot_dir = east;
+      dx = +1;
+      dy = 0;
+      break;
+  }
+
+  bool done = false;
+  while (!done)
+  {
+    // 1. drive straight until not a valid move (hit occupied cell or at end of map)
+    bool hitWall = false;
+    while(!hitWall) {
+      x2 += dx;
+      y2 += dy;
+      if (!validMove(x2, y2, nCols, nRows, grid, visited))
+      {
+        hitWall = true;
+        x2 = pathNodes.back().pos.x;
+        y2 = pathNodes.back().pos.y;
+        break;
+      }
+      if (!hitWall) {
+        addNodeToList(x2, y2, pathNodes, visited);
+      }
+    }
+
+    // 2. check left and right after hitting wall, then change direction
+    if (robot_dir == north || robot_dir == south)
+    {
+      if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited)
+        && !validMove(x2 - 1, y2, nCols, nRows, grid, visited)) {
+        // dead end, exit
+        done = true;
+        break;
+      } else if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited)) {
+        // east is occupied, travel towards west
+        x2--;
+        pattern_dir_ = west;
+      } else if (!validMove(x2 - 1, y2, nCols, nRows, grid, visited)) {
+        // west is occupied, travel towards east
+        x2++;
+        pattern_dir_ = east;
+      } else {
+        // both sides are opened. If don't have a prefered turn direction, travel towards most open direction
+        if (!(pattern_dir_ == east || pattern_dir_ == west)) {
+          if (validMove(x2, y2 + 1, nCols, nRows, grid, visited)) {
+            pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, north);        
+          } else {
+            pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, south);
+          }
+          ROS_INFO("rotation dir with most space successful");
+        }
+        if (pattern_dir_ = east) {
+            x2++;
+        } else if (pattern_dir_ = west) {
+            x2--;
+        }
+      }
+
+      // add Node to List
+      addNodeToList(x2, y2, pathNodes, visited);
+
+      // change direction 180 deg
+      if (robot_dir == north) {
+        robot_dir = south;
+        dy = -1;
+      } else if (robot_dir == south) {
+        robot_dir = north;
+        dy = 1;
+      }
+    }
+    else if (robot_dir == east || robot_dir == west)
+    {
+      if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited)
+        && !validMove(x2, y2 - 1, nCols, nRows, grid, visited)) {
+        // dead end, exit
+        done = true;
+        break;
+      } else if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited)) {
+        // north is occupied, travel towards south
+        y2--;
+        pattern_dir_ = south;
+      } else if (!validMove(x2, y2 - 1, nCols, nRows, grid, visited)) {
+        // south is occupied, travel towards north
+        y2++;
+        pattern_dir_ = north;
+      } else {
+        // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge
+        if (!(pattern_dir_ == north || pattern_dir_ == south)) {
+          if (validMove(x2 + 1, y2, nCols, nRows, grid, visited)) {
+            pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, east);        
+          } else {
+            pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, west);
+          }
+        }
+        if (pattern_dir_ = north) {
+            y2++;
+        } else if (pattern_dir_ = south) {
+            y2--;
+        }
+      }
+
+      // add Node to List
+      addNodeToList(x2, y2, pathNodes, visited);
+      
+      // change direction 180 deg
+      if (robot_dir == east) {
+        robot_dir = west;
+        dx = -1;
+      } else if (robot_dir == west) {
+        robot_dir = east;
+        dx = 1;
+      }
+    }
+  }
+  // Log
+  // printPathNodes(pathNodes);
+  return pathNodes;
+}
+
+std::list<Point_t> BoustrophedonSTC::boustrophedon_stc(std::vector<std::vector<bool> > const& grid,
+                                          Point_t& init,
+                                          int &multiple_pass_counter,
+                                          int &visited_counter)
+{
+  int x, y, nRows = grid.size(), nCols = grid[0].size();
+  pattern_dir_ = point;
+  // Initial node is initially set as visited so it does not count
+  multiple_pass_counter = 0;
+  visited_counter = 0;
+
+  std::vector<std::vector<bool>> visited;
+  visited = grid;  // Copy grid matrix
+  x = init.x;
+  y = init.y;
+
+  // add initial point to pathNodes
+  std::list<gridNode_t> pathNodes;
+  std::list<Point_t> fullPath;
+  addNodeToList(x, y, pathNodes, visited);
+
+  std::list<Point_t> goals = map_2_goals(visited, eNodeOpen);  // Retrieve all goalpoints (Cells not visited)
+  std::cout << "Goals Left: " << goals.size() << std::endl;
+  std::list<gridNode_t>::iterator it;
+
+#ifdef DEBUG_PLOT
+  ROS_INFO("Grid before walking is: ");
+  printGrid(grid, visited, fullPath);
+#endif
+
+  while (goals.size() != 0)
+  {
+    // boustrophedon pattern from current position
+    pathNodes = boustrophedon(grid, pathNodes, visited);
+#ifdef DEBUG_PLOT
+    ROS_INFO("Visited grid updated after boustrophedon:");
+    printGrid(grid, visited, pathNodes, PatternStart, pathNodes.back());
+#endif
+
+    for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
+    {
+      Point_t newPoint = { it->pos.x, it->pos.y };
+      visited_counter++;
+      fullPath.push_back(newPoint);
+    }
+
+    goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints
+
+    // Remove all elements from pathNodes list except last element.
+    // The last point is the starting point for a new search and A* extends the path from there on
+    pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
+    visited_counter--;  // First point is already counted as visited
+    // Plan to closest open Node using A*
+    // `goals` is essentially the map, so we use `goals` to determine the distance from the end of a potential path
+    //    to the nearest free space
+    bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes);
+    if (resign)
+    {
+      ROS_WARN("A_star_to_open_space is resigning! This may be due to the open cells outside of the obstacle boundary. Goals Left: %u", goals.size());
+      break;
+    }
+
+    // Update visited grid
+    for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
+    {
+      if (visited[it->pos.y][it->pos.x])
+      {
+        multiple_pass_counter++;
+      }
+      visited[it->pos.y][it->pos.x] = eNodeVisited;
+    }
+    if (pathNodes.size() > 0)
+    {
+      multiple_pass_counter--;  // First point is already counted as visited
+    }
+
+#ifdef DEBUG_PLOT
+    ROS_INFO("Grid with path marked as visited is:");
+    gridNode_t PatternStart = pathNodes.back();
+    printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back());
+#endif
+
+  }
+
+  return fullPath;
+}
+
+bool BoustrophedonSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
+                         std::vector<geometry_msgs::PoseStamped>& plan)
+{
+  if (!initialized_)
+  {
+    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
+    return false;
+  }
+  else
+  {
+    ROS_INFO("Initialized!");
+  }
+
+  //clear the plan, just in case
+  plan.clear();
+  costmap_ = costmap_ros_->getCostmap();
+
+  clock_t begin = clock();
+  Point_t startPoint;
+
+  /********************** Get grid from server **********************/
+  std::vector<std::vector<bool> > grid;
+  nav_msgs::GetMap grid_req_srv;
+  ROS_INFO("Requesting grid...");
+  if (!cpp_grid_client_.call(grid_req_srv))
+  {
+    ROS_ERROR("Could not retrieve grid from map_server");
+    return false;
+  }
+  ROS_INFO("grid recieved!!");
+
+  ROS_INFO("Parsing grid to internal representation...");
+  if (!parseCostmap(costmap_, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint))
+  {
+    ROS_ERROR("Could not parse retrieved grid");
+    return false;
+  }
+  ROS_INFO("grid parsed!!");
+
+#ifdef DEBUG_PLOT
+  ROS_INFO("Start grid is:");
+  std::list<Point_t> printPath;
+  printPath.push_back(startPoint);
+  printGrid(grid, grid, printPath);
+#endif
+
+  std::list<Point_t> goalPoints = boustrophedon_stc(grid,
+                                              startPoint,
+                                              boustrophedon_cpp_metrics_.multiple_pass_counter,
+                                              boustrophedon_cpp_metrics_.visited_counter);
+  ROS_INFO("naive cpp completed!");
+  ROS_INFO("Converting path to plan");
+
+  parsePointlist2Plan(start, goalPoints, plan);
+  // Print some metrics:
+  boustrophedon_cpp_metrics_.accessible_counter = boustrophedon_cpp_metrics_.visited_counter
+                                            - boustrophedon_cpp_metrics_.multiple_pass_counter;
+  boustrophedon_cpp_metrics_.total_area_covered = (4.0 * tool_radius_ * tool_radius_) * boustrophedon_cpp_metrics_.accessible_counter;
+  ROS_INFO("Total visited: %d", boustrophedon_cpp_metrics_.visited_counter);
+  ROS_INFO("Total re-visited: %d", boustrophedon_cpp_metrics_.multiple_pass_counter);
+  ROS_INFO("Total accessible cells: %d", boustrophedon_cpp_metrics_.accessible_counter);
+  ROS_INFO("Total accessible area: %f", boustrophedon_cpp_metrics_.total_area_covered);
+
+  // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept
+  // (also controlled by planner_frequency parameter in move_base namespace)
+
+  ROS_INFO("Publishing plan!");
+  publishPlan(plan);
+  ROS_INFO("Plan published!");
+  ROS_DEBUG("Plan published");
+
+  clock_t end = clock();
+  double elapsed_secs = static_cast<double>(end - begin) / CLOCKS_PER_SEC;
+  std::cout << "elapsed time: " << elapsed_secs << "\n";
+
+  return true;
+}
+}  // namespace full_coverage_path_planner
diff --git a/src/common.cpp b/src/common.cpp
index bc134a7..74077f2 100644
--- a/src/common.cpp
+++ b/src/common.cpp
@@ -309,3 +309,80 @@ std::list<Point_t> map_2_goals(std::vector<std::vector<bool> > const& grid, bool
   }
   return goals;
 }
+
+
+void printPathNodes(std::list<gridNode_t> pathNodes)
+{
+  for (gridNode_t node : pathNodes) {
+    std::cout << "(" << node.pos.x << ", " << node.pos.y << ")" << ": " << node.cost << " " << node.he << std::endl;
+  }
+  std:: cout << "--------------------------------------" << std::endl;
+
+}
+
+bool validMove(int x2, int y2, int nCols, int nRows,
+               std::vector<std::vector<bool>> const& grid,
+               std::vector<std::vector<bool>> const& visited)
+{
+  return (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) // path node is within the map
+        && (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen); // the path node is unvisited
+}
+
+void addNodeToList(int x2, int y2, std::list<gridNode_t>& pathNodes,
+                   std::vector<std::vector<bool>>& visited) {
+  Point_t new_point = { x2, y2 };
+  gridNode_t new_node =
+  {
+    new_point,  // Point: x,y
+    0,          // Cost
+    0,          // Heuristic
+  };
+  pathNodes.push_back(new_node);
+  visited[y2][x2] = eNodeVisited;  // Close node
+  return;
+}
+
+int dirWithMostSpace(int x_init, int y_init, int nCols, int nRows,
+                     std::vector<std::vector<bool> > const& grid,
+                     std::vector<std::vector<bool> > const& visited,
+                     int ignoreDir) {
+  // this array stores how far the robot can travel in a straight line for each direction
+  int free_space_in_dir[5] = {0};
+  // for each direction
+  for (int i = 1; i < 5; i++) {
+    // start from starting pos
+    int x2 = x_init;
+    int y2 = y_init;
+    do {  // loop until hit wall
+      switch (i) {
+        case east:
+          x2++;
+          break;
+        case west:
+          x2--;
+          break;
+        case north:
+          y2++;
+          break;
+        case south:
+          y2--;
+          break;
+        default:
+          break;
+      }
+      free_space_in_dir[i]++;
+    } while (validMove(x2, y2, nCols, nRows, grid, visited));
+  }
+
+  // set initial direction towards direction with most travel possible
+  int robot_dir = 0;
+  int indexValue = 0;
+  for (int i = 1; i <= 4; i++) {
+      // std::cout << "free space in " << i << ": " << free_space_in_dir[i] << std::endl;
+      if (free_space_in_dir[i] > indexValue && i != ignoreDir) {
+          robot_dir = i;
+          indexValue = free_space_in_dir[i];
+      }
+  }
+  return robot_dir;
+}
diff --git a/src/full_coverage_path_planner.cpp b/src/full_coverage_path_planner.cpp
index 0048682..c3c873b 100644
--- a/src/full_coverage_path_planner.cpp
+++ b/src/full_coverage_path_planner.cpp
@@ -185,6 +185,73 @@ void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamp
   ROS_INFO("Plan ready containing %lu goals!", plan.size());
 }
 
+bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_,
+                                           std::vector<std::vector<bool> >& grid,
+                                           float robotRadius,
+                                           float toolRadius,
+                                           geometry_msgs::PoseStamped const& realStart,
+                                           Point_t& scaledStart)
+{
+  int ix, iy, nodeRow, nodeCol;
+  int nodeSize = dmax(floor(toolRadius / costmap_grid_->getResolution()), 1);  // Size of node in pixels/units
+  int robotNodeSize = dmax(floor(robotRadius / costmap_grid_->getResolution()), 1);  // RobotRadius in pixels/units
+  int nodePaddingSize = dmax(ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0), 0);
+  int nRows = costmap_grid_->getSizeInCellsY(), nCols = costmap_grid_->getSizeInCellsX();
+  ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize);
+  ROS_INFO("parseCostmap -> robotNodeSize: %u nodePaddingSize: %u", robotNodeSize, nodePaddingSize);
+  ROS_INFO("parseCostmap -> robotRadius: %f toolRadius: %f", robotRadius, toolRadius);
+
+  if (nRows == 0 || nCols == 0)
+  {
+    return false;
+  }
+
+  // Save map origin and scaling
+  tile_size_ = nodeSize * costmap_grid_->getResolution();  // Size of a tile in meters
+  grid_origin_.x = costmap_grid_->getOriginX();  // x-origin in meters
+  grid_origin_.y = costmap_grid_->getOriginY();  // y-origin in meters
+  ROS_INFO("costmap resolution: %g", costmap_grid_->getResolution());
+  ROS_INFO("tile size: %g", tile_size_);
+  ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y);
+
+  // Scale starting point
+  scaledStart.x = static_cast<unsigned int>(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0,
+                             floor(nCols / tile_size_)));
+  scaledStart.y = static_cast<unsigned int>(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0,
+                             floor(nRows / tile_size_)));
+  ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y);
+  ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y);
+
+  // Scale grid
+  for (iy = 0; iy < nRows; iy = iy + nodeSize)
+  {
+    std::vector<bool> gridRow;
+    for (ix = 0; ix < nCols; ix = ix + nodeSize)
+    {
+      bool nodeOccupied = false;
+      for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy - nodePaddingSize + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow)
+      {
+        if ((iy - nodePaddingSize + nodeRow) < 0) {continue;}
+        for (nodeCol = 0; (nodeCol < robotNodeSize) && ((ix - nodePaddingSize + nodeCol) < nCols); ++nodeCol)
+        {
+          if ((ix - nodePaddingSize + nodeCol) < 0) {continue;}
+          double mx = ix - nodePaddingSize + nodeCol;
+          double my = iy - nodePaddingSize + nodeRow;
+          if (costmap_grid_->getCost(mx, my) > costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+          {
+            nodeOccupied = true;
+            // ROS_INFO("(%f, %f) marked occupied", mx, my);
+            break;
+          }
+        }
+      }
+      gridRow.push_back(nodeOccupied);
+    }
+    grid.push_back(gridRow);
+  }
+  return true;
+}
+
 bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_,
                                         std::vector<std::vector<bool> >& grid,
                                         float robotRadius,
@@ -207,12 +274,17 @@ bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_
   tile_size_ = nodeSize * cpp_grid_.info.resolution;  // Size of a tile in meters
   grid_origin_.x = cpp_grid_.info.origin.position.x;  // x-origin in meters
   grid_origin_.y = cpp_grid_.info.origin.position.y;  // y-origin in meters
+  ROS_INFO("costmap resolution: %g", cpp_grid_.info.resolution);
+  ROS_INFO("tile size: %g", tile_size_);
+  ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y);
 
   // Scale starting point
   scaledStart.x = static_cast<unsigned int>(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0,
                              floor(cpp_grid_.info.width / tile_size_)));
   scaledStart.y = static_cast<unsigned int>(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0,
                              floor(cpp_grid_.info.height / tile_size_)));
+  ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y);
+  ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y);
 
   // Scale grid
   for (iy = 0; iy < nRows; iy = iy + nodeSize)
diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp
deleted file mode 100644
index 33e9c33..0000000
--- a/src/spiral_stc.cpp
+++ /dev/null
@@ -1,284 +0,0 @@
-//
-// Copyright [2020] Nobleo Technology"  [legal/copyright]
-//
-#include <algorithm>
-#include <iostream>
-#include <list>
-#include <string>
-#include <vector>
-
-#include "full_coverage_path_planner/spiral_stc.h"
-#include <pluginlib/class_list_macros.h>
-
-// register this planner as a BaseGlobalPlanner plugin
-PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::SpiralSTC, nav_core::BaseGlobalPlanner)
-
-namespace full_coverage_path_planner
-{
-void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
-{
-  if (!initialized_)
-  {
-    // Create a publisher to visualize the plan
-    ros::NodeHandle private_nh("~/");
-    ros::NodeHandle nh, private_named_nh("~/" + name);
-
-    plan_pub_ = private_named_nh.advertise<nav_msgs::Path>("plan", 1);
-    // Try to request the cpp-grid from the cpp_grid map_server
-    cpp_grid_client_ = nh.serviceClient<nav_msgs::GetMap>("static_map");
-
-    // Define  robot radius (radius) parameter
-    float robot_radius_default = 0.5f;
-    private_named_nh.param<float>("robot_radius", robot_radius_, robot_radius_default);
-    // Define  tool radius (radius) parameter
-    float tool_radius_default = 0.5f;
-    private_named_nh.param<float>("tool_radius", tool_radius_, tool_radius_default);
-    initialized_ = true;
-  }
-}
-
-std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const& grid, std::list<gridNode_t>& init,
-                                        std::vector<std::vector<bool> >& visited)
-{
-  int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size();
-  // Spiral filling of the open space
-  // Copy incoming list to 'end'
-  std::list<gridNode_t> pathNodes(init);
-  // Create iterator for gridNode_t list and let it point to the last element of end
-  std::list<gridNode_t>::iterator it = --(pathNodes.end());
-  if (pathNodes.size() > 1)  // if list is length 1, keep iterator at end
-    it--;                    // Let iterator point to second to last element
-
-  gridNode_t prev = *(it);
-  bool done = false;
-  while (!done)
-  {
-    if (it != pathNodes.begin())
-    {
-      // turn ccw
-      dx = pathNodes.back().pos.x - prev.pos.x;
-      dy = pathNodes.back().pos.y - prev.pos.y;
-      dx_prev = dx;
-      dx = -dy;
-      dy = dx_prev;
-    }
-    else
-    {
-      // Initialize spiral direction towards y-axis
-      dx = 0;
-      dy = 1;
-    }
-    done = true;
-
-    for (int i = 0; i < 4; ++i)
-    {
-      x2 = pathNodes.back().pos.x + dx;
-      y2 = pathNodes.back().pos.y + dy;
-      if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows)
-      {
-        if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen)
-        {
-          Point_t new_point = { x2, y2 };
-          gridNode_t new_node =
-          {
-            new_point,  // Point: x,y
-            0,          // Cost
-            0,          // Heuristic
-          };
-          prev = pathNodes.back();
-          pathNodes.push_back(new_node);
-          it = --(pathNodes.end());
-          visited[y2][x2] = eNodeVisited;  // Close node
-          done = false;
-          break;
-        }
-      }
-      // try next direction cw
-      dx_prev = dx;
-      dx = dy;
-      dy = -dx_prev;
-    }
-  }
-  return pathNodes;
-}
-
-std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const& grid,
-                                          Point_t& init,
-                                          int &multiple_pass_counter,
-                                          int &visited_counter)
-{
-  int x, y, nRows = grid.size(), nCols = grid[0].size();
-  // Initial node is initially set as visited so it does not count
-  multiple_pass_counter = 0;
-  visited_counter = 0;
-
-  std::vector<std::vector<bool> > visited;
-  visited = grid;  // Copy grid matrix
-  x = init.x;
-  y = init.y;
-
-  Point_t new_point = { x, y };
-  gridNode_t new_node =
-  {
-    new_point,  // Point: x,y
-    0,          // Cost
-    0,          // Heuristic
-  };
-  std::list<gridNode_t> pathNodes;
-  std::list<Point_t> fullPath;
-  pathNodes.push_back(new_node);
-  visited[y][x] = eNodeVisited;
-
-#ifdef DEBUG_PLOT
-  ROS_INFO("Grid before walking is: ");
-  printGrid(grid, visited, fullPath);
-#endif
-
-  pathNodes = SpiralSTC::spiral(grid, pathNodes, visited);                // First spiral fill
-  std::list<Point_t> goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints
-  // Add points to full path
-  std::list<gridNode_t>::iterator it;
-  for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
-  {
-    Point_t newPoint = { it->pos.x, it->pos.y };
-    visited_counter++;
-    fullPath.push_back(newPoint);
-  }
-  // Remove all elements from pathNodes list except last element
-  pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
-
-#ifdef DEBUG_PLOT
-  ROS_INFO("Current grid after first spiral is");
-  printGrid(grid, visited, fullPath);
-  ROS_INFO("There are %d goals remaining", goals.size());
-#endif
-  while (goals.size() != 0)
-  {
-    // Remove all elements from pathNodes list except last element.
-    // The last point is the starting point for a new search and A* extends the path from there on
-    pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
-    visited_counter--;  // First point is already counted as visited
-    // Plan to closest open Node using A*
-    // `goals` is essentially the map, so we use `goals` to determine the distance from the end of a potential path
-    //    to the nearest free space
-    bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes);
-    if (resign)
-    {
-#ifdef DEBUG_PLOT
-      ROS_INFO("A_star_to_open_space is resigning", goals.size());
-#endif
-      break;
-    }
-
-    // Update visited grid
-    for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
-    {
-      if (visited[it->pos.y][it->pos.x])
-      {
-        multiple_pass_counter++;
-      }
-      visited[it->pos.y][it->pos.x] = eNodeVisited;
-    }
-    if (pathNodes.size() > 0)
-    {
-      multiple_pass_counter--;  // First point is already counted as visited
-    }
-
-#ifdef DEBUG_PLOT
-    ROS_INFO("Grid with path marked as visited is:");
-    gridNode_t SpiralStart = pathNodes.back();
-    printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back());
-#endif
-
-    // Spiral fill from current position
-    pathNodes = spiral(grid, pathNodes, visited);
-
-#ifdef DEBUG_PLOT
-    ROS_INFO("Visited grid updated after spiral:");
-    printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back());
-#endif
-
-    goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints
-
-    for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
-    {
-      Point_t newPoint = { it->pos.x, it->pos.y };
-      visited_counter++;
-      fullPath.push_back(newPoint);
-    }
-  }
-
-  return fullPath;
-}
-
-bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
-                         std::vector<geometry_msgs::PoseStamped>& plan)
-{
-  if (!initialized_)
-  {
-    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
-    return false;
-  }
-  else
-  {
-    ROS_INFO("Initialized!");
-  }
-
-  clock_t begin = clock();
-  Point_t startPoint;
-
-  /********************** Get grid from server **********************/
-  std::vector<std::vector<bool> > grid;
-  nav_msgs::GetMap grid_req_srv;
-  ROS_INFO("Requesting grid!!");
-  if (!cpp_grid_client_.call(grid_req_srv))
-  {
-    ROS_ERROR("Could not retrieve grid from map_server");
-    return false;
-  }
-
-  if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint))
-  {
-    ROS_ERROR("Could not parse retrieved grid");
-    return false;
-  }
-
-#ifdef DEBUG_PLOT
-  ROS_INFO("Start grid is:");
-  std::list<Point_t> printPath;
-  printPath.push_back(startPoint);
-  printGrid(grid, grid, printPath);
-#endif
-
-  std::list<Point_t> goalPoints = spiral_stc(grid,
-                                              startPoint,
-                                              spiral_cpp_metrics_.multiple_pass_counter,
-                                              spiral_cpp_metrics_.visited_counter);
-  ROS_INFO("naive cpp completed!");
-  ROS_INFO("Converting path to plan");
-
-  parsePointlist2Plan(start, goalPoints, plan);
-  // Print some metrics:
-  spiral_cpp_metrics_.accessible_counter = spiral_cpp_metrics_.visited_counter
-                                            - spiral_cpp_metrics_.multiple_pass_counter;
-  spiral_cpp_metrics_.total_area_covered = (4.0 * tool_radius_ * tool_radius_) * spiral_cpp_metrics_.accessible_counter;
-  ROS_INFO("Total visited: %d", spiral_cpp_metrics_.visited_counter);
-  ROS_INFO("Total re-visited: %d", spiral_cpp_metrics_.multiple_pass_counter);
-  ROS_INFO("Total accessible cells: %d", spiral_cpp_metrics_.accessible_counter);
-  ROS_INFO("Total accessible area: %f", spiral_cpp_metrics_.total_area_covered);
-
-  // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept
-  // (also controlled by planner_frequency parameter in move_base namespace)
-
-  ROS_INFO("Publishing plan!");
-  publishPlan(plan);
-  ROS_INFO("Plan published!");
-  ROS_DEBUG("Plan published");
-
-  clock_t end = clock();
-  double elapsed_secs = static_cast<double>(end - begin) / CLOCKS_PER_SEC;
-  std::cout << "elapsed time: " << elapsed_secs << "\n";
-
-  return true;
-}
-}  // namespace full_coverage_path_planner
diff --git a/test/README.md b/test/README.md
index 1f9b09b..c7c3ea8 100644
--- a/test/README.md
+++ b/test/README.md
@@ -5,10 +5,10 @@ The full coverage path planner consists of several parts that are each tested se
 
 The move_base_flex plugin consists of several parts, each unit-tested separately:
 - test_common: tests common.h
-- test_spiral_stc: tests static functions of spiral_stc.h
+- test_boustrophedon_stc: tests static functions of boustrophedon_stc.h
 
 Besides unittests, there are also some launch files that both illustrate how to use the
-- SpiralSTC-plugin, in test/full_coverage_path_planner/test_full_coverage_path_planner.launch
+- BoustrophedonSTC-plugin, in test/full_coverage_path_planner/test_full_coverage_path_planner.launch
 
 Note that the .launch-files do not do any automatic testing or verification of anything,
 they are there to make manual testing easier.
diff --git a/test/full_coverage_path_planner/fcpp.rviz b/test/full_coverage_path_planner/fcpp.rviz
index c052c0b..5170835 100644
--- a/test/full_coverage_path_planner/fcpp.rviz
+++ b/test/full_coverage_path_planner/fcpp.rviz
@@ -106,7 +106,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.019999999552965164
       Shaft Length: 0.05000000074505806
-      Topic: /move_base/SpiralSTC/plan
+      Topic: /move_base/BoustrophedonSTC/plan
       Unreliable: false
       Value: true
     - Angle Tolerance: 0.10000000149011612
@@ -247,4 +247,4 @@ Window Geometry:
     collapsed: true
   Width: 927
   X: 67
-  Y: 27
+  Y: 27
\ No newline at end of file
diff --git a/test/full_coverage_path_planner/move_base_sim.launch b/test/full_coverage_path_planner/move_base_sim.launch
new file mode 100644
index 0000000..27cc156
--- /dev/null
+++ b/test/full_coverage_path_planner/move_base_sim.launch
@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+
+<launch>
+    <arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
+    <arg name="coverage_area_offset" default="-2.5 -2.5 0 0 0 0"/>
+    <arg name="coverage_area_size_x" default="10"/>
+    <arg name="coverage_area_size_y" default="10"/>
+    <arg name="target_x_vel" default="0.5"/>
+    <arg name="target_yaw_vel" default="0.4"/>
+    <arg name="robot_radius" default="0.3"/>
+    <arg name="tool_radius" default="0.3"/>
+    <arg name="rviz" default="true"/>
+
+    <!-- Mobile robot simulator -->
+    <node pkg="mobile_robot_simulator" type="mobile_robot_simulator_node" name="mobile_robot_simulator" output="screen">
+        <param name="publish_map_transform" value="true"/>
+        <param name="publish_rate" value="10.0"/>
+        <param name="velocity_topic" value="/cmd_vel"/>
+        <param name="odometry_topic" value="/odom"/>
+    </node>
+
+    <!--Run the map server-->
+    <node name="grid_server" pkg="map_server" type="map_server" args="$(arg map)">
+        <param name="frame_id" value="map"/>
+    </node>
+
+    <!--Tracking_pid cannot just accept a nav_msgs/Path, it can only go to a PoseStamped,
+       so the path_interpolator drags a PoseStamped over a Path at a given speed-->
+    <node name="interpolator" pkg="tracking_pid" type="path_interpolator">
+        <param name="target_x_vel" value="$(arg target_x_vel)"/>
+        <param name="target_yaw_vel" value="$(arg target_yaw_vel)"/>
+        <!-- <remap from="path" to="/move_base/BoustrophedonSTC/plan"/> -->
+    </node>
+
+    <!--Tracking_pid tries to get the robot as close to it's goal point as possible-->
+    <node name="controller" pkg="tracking_pid" type="controller" output="screen">
+        <remap from="move_base/cmd_vel" to="/cmd_vel"/>
+        <remap from="local_trajectory" to="trajectory"/>
+        <param name="controller_debug_enabled" value="True"/>
+        <param name="track_base_link" value="true"/>
+        <param name="l" value="0.5"/>
+        <param name="Ki_long" value="0.0"/>
+        <param name="Kp_long" value="2.0"/>
+        <param name="Kd_long" value="0.5"/>
+        <param name="Ki_lat" value="0.0"/>
+        <param name="Kp_lat" value="4.0"/>
+        <param name="Kd_lat" value="0.3"/>
+    </node>
+
+    <!-- Launch coverage progress tracking -->
+    <node pkg="tf" type="static_transform_publisher" name="map_to_coveragemap" args="$(arg coverage_area_offset) map coverage_map 100" />
+    <node pkg="full_coverage_path_planner" type="coverage_progress" name="coverage_progress">
+        <param name="~target_area/x" value="$(arg coverage_area_size_x)" />
+        <param name="~target_area/y" value="$(arg coverage_area_size_y)" />
+        <param name="~coverage_radius" value="$(arg tool_radius)" />
+        <remap from="reset" to="coverage_progress/reset" />
+        <param name="~map_frame" value="/coverage_map"/>
+    </node>
+
+    <!-- rviz -->
+    <node if="$(eval rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(find full_coverage_path_planner)/test/full_coverage_path_planner/fcpp.rviz" />
+</launch>
diff --git a/test/full_coverage_path_planner/param/costmap_common_params.yaml b/test/full_coverage_path_planner/param/costmap_common_params.yaml
new file mode 100644
index 0000000..a689051
--- /dev/null
+++ b/test/full_coverage_path_planner/param/costmap_common_params.yaml
@@ -0,0 +1,19 @@
+transform_tolerance: 0.2
+
+map_topic: map
+
+footprint: [[0.46, 0.28], [-0.50, 0.28], [-0.50, -0.28], [0.46, -0.28]]
+footprint_padding: 0.05
+inflation_radius: 0.55
+
+#observation_sources: point_cloud_sensor
+#obstacle_range: 2.5
+#raytrace_range: 3.0
+#point_cloud_sensor: { sensor_frame: d435i_link,
+#                      data_type: PointCloud2,
+#                      topic: /points_filtered,
+#                      marking: true,
+#                      clearing: true,
+#                      expected_update_rate: 1.0
+#                      max_obstacle_height: 0.75
+#                      min_obstacle_height: 0.05 }
\ No newline at end of file
diff --git a/test/full_coverage_path_planner/param/global_costmap_params.yaml b/test/full_coverage_path_planner/param/global_costmap_params.yaml
new file mode 100644
index 0000000..e9a0668
--- /dev/null
+++ b/test/full_coverage_path_planner/param/global_costmap_params.yaml
@@ -0,0 +1,7 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 1.0
+  static_map: true
+  always_send_full_costmap: true
\ No newline at end of file
diff --git a/test/full_coverage_path_planner/param/local_costmap_params.yaml b/test/full_coverage_path_planner/param/local_costmap_params.yaml
index 5147ed1..821db1b 100644
--- a/test/full_coverage_path_planner/param/local_costmap_params.yaml
+++ b/test/full_coverage_path_planner/param/local_costmap_params.yaml
@@ -1,10 +1,10 @@
 local_costmap:
    global_frame: odom
    robot_base_frame: base_link
-   update_frequency: 10.0
+   update_frequency: 5.0
    publish_frequency: 5.0
    width: 5.0
    height: 5.0
    resolution: 0.05
    static_map: false
-   rolling_window: true
+   rolling_window: true
\ No newline at end of file
diff --git a/test/full_coverage_path_planner/param/planners.yaml b/test/full_coverage_path_planner/param/planners.yaml
index a59da1f..d47b710 100644
--- a/test/full_coverage_path_planner/param/planners.yaml
+++ b/test/full_coverage_path_planner/param/planners.yaml
@@ -1,3 +1,3 @@
 planners:
-  - name: 'SpiralSTC'
-    type: 'full_coverage_path_planner/SpiralSTC'
+  - name: 'BoustrophedonSTC'
+    type: 'full_coverage_path_planner/BoustrophedonSTC'
diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch
index ffa9c89..91d81c2 100644
--- a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch
+++ b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch
@@ -1,34 +1,39 @@
 <?xml version="1.0"?>
 
 <launch>
-    <arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
+    <arg name="map" default="$(find full_coverage_path_planner)/maps/square.yaml"/>
     <arg name="coverage_area_offset" default="-2.5 -2.5 0 0 0 0"/>
     <arg name="coverage_area_size_x" default="10"/>
     <arg name="coverage_area_size_y" default="10"/>
     <arg name="target_x_vel" default="0.5"/>
     <arg name="target_yaw_vel" default="0.4"/>
     <arg name="robot_radius" default="0.3"/>
-    <arg name="tool_radius" default="0.3"/>
+    <arg name="tool_radius" default="0.2"/>
     <arg name="rviz" default="true"/>
 
     <!--Move base flex, using the full_coverage_path_planner-->
     <node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="screen" required="true">
         <param name="tf_timeout" value="1.5"/>
         <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/planners.yaml" command="load" />
+        <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/costmap_common_params.yaml" command="load" ns="global_costmap"/>
+        <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/costmap_common_params.yaml" command="load" ns="local_costmap"/>
+        <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/global_costmap_params.yaml" command="load" />
         <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/local_costmap_params.yaml" command="load" />
-        <param name="SpiralSTC/robot_radius" value="$(arg robot_radius)"/>
-        <param name="SpiralSTC/tool_radius" value="$(arg tool_radius)"/>
+        <param name="BoustrophedonSTC/robot_radius" value="$(arg robot_radius)"/>
+        <param name="BoustrophedonSTC/tool_radius" value="$(arg tool_radius)"/>
         <param name="global_costmap/robot_radius" value="$(arg robot_radius)"/>
+<!--        <param name="planner_frequency" value="2"/>-->
         <remap from="odom" to="/odom"/>
         <remap from="scan" to="/scan"/>
 
-        <remap from="/move_base_flex/SpiralSTC/plan" to="/move_base/SpiralSTC/plan"/>
+        <remap from="/move_base_flex/BoustrophedonSTC/plan" to="/move_base/BoustrophedonSTC/plan"/>
         <remap from="/move_base_flex/tracking_pid/interpolator" to="/move_base/TrackingPidLocalPlanner/interpolator"/>
     </node>
 
     <!-- Move Base backwards compatibility -->
     <node pkg="mbf_costmap_nav" type="move_base_legacy_relay.py" name="move_base" >
-        <param name="base_global_planner" value="SpiralSTC" />
+        <param name="base_global_planner" value="BoustrophedonSTC" />
+        <param name="planner_frequency" value="2"/>
     </node>
 
     <!-- Mobile robot simulator -->
@@ -49,7 +54,7 @@
     <node name="interpolator" pkg="tracking_pid" type="path_interpolator">
         <param name="target_x_vel" value="$(arg target_x_vel)"/>
         <param name="target_yaw_vel" value="$(arg target_yaw_vel)"/>
-        <remap from="path" to="/move_base/SpiralSTC/plan"/>
+        <remap from="path" to="/move_base/BoustrophedonSTC/plan"/>
     </node>
 
     <!--Tracking_pid tries to get the robot as close to it's goal point as possible-->
diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch
index 97d0d9d..d61efd5 100644
--- a/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch
+++ b/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 
 <launch>
-    <arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
+    <arg name="map" default="$(find full_coverage_path_planner)/maps/square.yaml"/>
     <arg name="robot_radius" default="0.3"/>
     <arg name="tool_radius" default="0.2"/>
 
@@ -11,13 +11,13 @@
         <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/planners.yaml" command="load" />
         <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/local_costmap_params.yaml" command="load" />
 
-        <param name="SpiralSTC/robot_radius" value="$(arg robot_radius)"/>
-        <param name="SpiralSTC/tool_radius" value="$(arg tool_radius)"/>
+        <param name="BoustrophedonSTC/robot_radius" value="$(arg robot_radius)"/>
+        <param name="BoustrophedonSTC/tool_radius" value="$(arg tool_radius)"/>
         <param name="global_costmap/robot_radius" value="$(arg robot_radius)"/>
         <remap from="odom" to="/odom"/>
         <remap from="scan" to="/scan"/>
 
-        <remap from="/move_base_flex/SpiralSTC/plan" to="/move_base/SpiralSTC/plan"/>
+        <remap from="/move_base_flex/BoustrophedonSTC/plan" to="/move_base/BoustrophedonSTC/plan"/>
         <remap from="/move_base_flex/tracking_pid/interpolator" to="/move_base/TrackingPidLocalPlanner/interpolator"/>
     </node>
     <!-- Move Base backwards compatibility -->
diff --git a/test/src/test_spiral_stc.cpp b/test/src/test_boustrophedon_stc.cpp
similarity index 89%
rename from test/src/test_spiral_stc.cpp
rename to test/src/test_boustrophedon_stc.cpp
index 8335255..5d23a07 100644
--- a/test/src/test_spiral_stc.cpp
+++ b/test/src/test_boustrophedon_stc.cpp
@@ -24,7 +24,7 @@
 #include <ros/ros.h>
 
 #include <full_coverage_path_planner/common.h>
-#include <full_coverage_path_planner/spiral_stc.h>
+#include <full_coverage_path_planner/boustrophedon_stc.h>
 #include <full_coverage_path_planner/util.h>
 
 cv::Mat drawMap(std::vector<std::vector<bool> > const& grid);
@@ -35,15 +35,15 @@ cv::Mat drawPath(const cv::Mat &mapImg,
                  std::list<Point_t> &path);
 
 /*
- * On a map with nothing on it, spiral_stc should cover all the nodes of the map
+ * On a map with nothing on it, boustrophedon_stc should cover all the nodes of the map
  */
-TEST(TestSpiralStc, testFillEmptyMap)
+TEST(TestBoustrophedonStc, testFillEmptyMap)
 {
   std::vector<std::vector<bool> > grid = makeTestGrid(4, 4, false);
 
   Point_t start = {0, 0};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -54,7 +54,7 @@ TEST(TestSpiralStc, testFillEmptyMap)
 /*
  * On a map with a single obstacle, all the other nodes should still be visited.
  */
-TEST(TestSpiralStc, testFillMapWithOneObstacle)
+TEST(TestBoustrophedonStc, testFillMapWithOneObstacle)
 {
   /*
    * [s 0 0 0]
@@ -67,7 +67,7 @@ TEST(TestSpiralStc, testFillMapWithOneObstacle)
 
   Point_t start = {0, 0};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -82,7 +82,7 @@ TEST(TestSpiralStc, testFillMapWithOneObstacle)
 /*
  * In a map with 2 obstacles, still the complete map should be covered except for those 2 obstacles
  */
-TEST(TestSpiralStc, testFillMapWith2Obstacles)
+TEST(TestBoustrophedonStc, testFillMapWith2Obstacles)
 {
   /*
    * [s 0 0 0]
@@ -96,7 +96,7 @@ TEST(TestSpiralStc, testFillMapWith2Obstacles)
 
   Point_t start = {0, 0};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -111,7 +111,7 @@ TEST(TestSpiralStc, testFillMapWith2Obstacles)
 /*
  * On a 4x4 map where the opposite right half of the map is blocked, we can cover only the 4x2 reachable nodes
  */
-TEST(TestSpiralStc, testFillMapWithHalfBlocked)
+TEST(TestBoustrophedonStc, testFillMapWithHalfBlocked)
 {
   /*
    * [s 0 1 0]
@@ -127,7 +127,7 @@ TEST(TestSpiralStc, testFillMapWithHalfBlocked)
 
   Point_t start = {0, 0};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -141,9 +141,9 @@ TEST(TestSpiralStc, testFillMapWithHalfBlocked)
 
 /*
  * On a map with a wall almost blocking off a half of the map, but leaving a gap to the other side,
- *  spiral_stc should still cover all reachable nodes
+ *  boustrophedon_stc should still cover all reachable nodes
  */
-TEST(TestSpiralStc, testFillMapWithWall)
+TEST(TestBoustrophedonStc, testFillMapWithWall)
 {
   /*
    * [s 0 1 0]
@@ -158,7 +158,7 @@ TEST(TestSpiralStc, testFillMapWithWall)
 
   Point_t start = {0, 0};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -173,7 +173,7 @@ TEST(TestSpiralStc, testFillMapWithWall)
 /*
  * This test case features a very short dead-end
  */
-TEST(TestSpiralStc, testDeadEnd1)
+TEST(TestBoustrophedonStc, testDeadEnd1)
 {
   /*
    * [0 0 1 0]
@@ -189,7 +189,7 @@ TEST(TestSpiralStc, testDeadEnd1)
 
   Point_t start = {1, 2};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -208,10 +208,10 @@ TEST(TestSpiralStc, testDeadEnd1)
 /*
  * This test case is an extension of testDeadEnd1, where the top row is also covered as an obstacle.
  * The top row is covered but the obstacle from testDeadEnd1 is shifted downwards
- *  in an attempt to see if SpiralSTC also fails when a dead-end is not on the edge of the map
+ *  in an attempt to see if BoustrophedonSTC also fails when a dead-end is not on the edge of the map
  *  (but below a row of obstacles)
  */
-TEST(TestSpiralStc, testDeadEnd1WithTopRow)
+TEST(TestBoustrophedonStc, testDeadEnd1WithTopRow)
 {
   /*
    * [1 1 1 1]
@@ -233,7 +233,7 @@ TEST(TestSpiralStc, testDeadEnd1WithTopRow)
 
   Point_t start = {1, 3};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -252,7 +252,7 @@ TEST(TestSpiralStc, testDeadEnd1WithTopRow)
 /*
  * This test case features a very short dead-end
  */
-TEST(TestSpiralStc, testDeadEnd2)
+TEST(TestBoustrophedonStc, testDeadEnd2)
 {
   /*
    * [1 0 0 0]
@@ -268,7 +268,7 @@ TEST(TestSpiralStc, testDeadEnd2)
 
   Point_t start = {3, 0};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -287,7 +287,7 @@ TEST(TestSpiralStc, testDeadEnd2)
 /*
  * This test case features a very short dead-end
  */
-TEST(TestSpiralStc, testDeadEnd3)
+TEST(TestBoustrophedonStc, testDeadEnd3)
 {
   /*
    * [0 0 0 0 0 0 1 0 0]
@@ -309,7 +309,7 @@ TEST(TestSpiralStc, testDeadEnd3)
 
   Point_t start = {5, 2};
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -328,7 +328,7 @@ TEST(TestSpiralStc, testDeadEnd3)
 /*
  * This test case features a very short dead-end
  */
-TEST(TestSpiralStc, testDeadEnd3WithTopRow)
+TEST(TestBoustrophedonStc, testDeadEnd3WithTopRow)
 {
   /*
    * [1 1 1 1 1 1 1 1 1]
@@ -359,7 +359,7 @@ TEST(TestSpiralStc, testDeadEnd3WithTopRow)
 
   Point_t start = {5, 3};  // NOLINT
   int multiple_pass_counter, visited_counter;
-  std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+  std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                               start,
                                                                               multiple_pass_counter,
                                                                               visited_counter);
@@ -496,7 +496,7 @@ Point_t findStart(std::vector<std::vector<bool> > const& grid)
  5. On each coordinate in path, fill pixel at that coordinate in pathImg
  6. pathImg and floodfilledImg should be identical
  */
-TEST(TestSpiralStc, testRandomMap)
+TEST(TestBoustrophedonStc, testRandomMap)
 {
   // Seed pseudorandom sequence to create *reproducible test
   unsigned int seed = 12345;
@@ -515,7 +515,7 @@ TEST(TestSpiralStc, testRandomMap)
     cv::Mat mapImg = drawMap(grid);
     Point_t start = findStart(grid);
     int multiple_pass_counter, visited_counter;
-    std::list<Point_t> path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid,
+    std::list<Point_t> path = full_coverage_path_planner::BoustrophedonSTC::boustrophedon_stc(grid,
                                                                                 start,
                                                                                 multiple_pass_counter,
                                                                                 visited_counter);