Fix path too close to obstacles issue

.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

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

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>

include/full_coverage_path_planner/spiral_stc.h → 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

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

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_;
 };
 
 

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

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>