Rename Spiral to Boustrophedon and Updated README.md accordingly

Author: ethanckim

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)
 

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

doc/fcpp_modified_boustrophedon.png

@@ -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>

fcpp_plugin.xml

@@ -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

include/full_coverage_path_planner/spiral_stc.h renamed to include/full_coverage_path_planner/boustrophedon_stc.h

@@ -79,7 +79,7 @@ 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,
@@ -130,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_;
 };
 
 

include/full_coverage_path_planner/full_coverage_path_planner.h

@@ -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>

package.xml

@@ -7,17 +7,17 @@
 #include <string>
 #include <vector>
 
-#include "full_coverage_path_planner/spiral_stc.h"
+#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::SpiralSTC, nav_core::BaseGlobalPlanner)
+PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::BoustrophedonSTC, nav_core::BaseGlobalPlanner)
 
 int pattern_dir_ = point;
 
 namespace full_coverage_path_planner
 {
-void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
+void BoustrophedonSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
 {
   if (!initialized_)
   {
@@ -42,7 +42,7 @@ void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_r
   }
 }
 
-std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const& grid, std::list<gridNode_t>& init,
+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();
@@ -74,7 +74,7 @@ std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const& g
       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 spiral_stc.cpp. Will travel east for now.");
+      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;
@@ -195,7 +195,7 @@ std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const& g
   return pathNodes;
 }
 
-std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const& grid,
+std::list<Point_t> BoustrophedonSTC::boustrophedon_stc(std::vector<std::vector<bool> > const& grid,
                                           Point_t& init,
                                           int &multiple_pass_counter,
                                           int &visited_counter)
@@ -228,10 +228,10 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
   while (goals.size() != 0)
   {
     // boustrophedon pattern from current position
-    pathNodes = spiral(grid, pathNodes, visited);
+    pathNodes = boustrophedon(grid, pathNodes, visited);
 #ifdef DEBUG_PLOT
-    ROS_INFO("Visited grid updated after spiral:");
-    printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back());
+    ROS_INFO("Visited grid updated after boustrophedon:");
+    printGrid(grid, visited, pathNodes, PatternStart, pathNodes.back());
 #endif
 
     for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
@@ -273,7 +273,7 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
 
 #ifdef DEBUG_PLOT
     ROS_INFO("Grid with path marked as visited is:");
-    gridNode_t SpiralStart = pathNodes.back();
+    gridNode_t PatternStart = pathNodes.back();
     printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back());
 #endif
 
@@ -282,7 +282,7 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
   return fullPath;
 }
 
-bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
+bool BoustrophedonSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                          std::vector<geometry_msgs::PoseStamped>& plan)
 {
   if (!initialized_)
@@ -328,22 +328,22 @@ bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry
   printGrid(grid, grid, printPath);
 #endif
 
-  std::list<Point_t> goalPoints = spiral_stc(grid,
+  std::list<Point_t> goalPoints = boustrophedon_stc(grid,
                                               startPoint,
-                                              spiral_cpp_metrics_.multiple_pass_counter,
-                                              spiral_cpp_metrics_.visited_counter);
+                                              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:
-  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);
+  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)

src/spiral_stc.cpp renamed to src/boustrophedon_stc.cpp

@@ -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.

test/README.md

@@ -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