├── src ├── full_coverage_path_planner │ ├── __init__.py │ └── stroke_joins.py ├── ant.cpp ├── full_coverage_path_planner.cpp ├── uturn.cpp ├── spiral_stc.cpp ├── common.cpp └── aco.cpp ├── doc ├── bsa.png ├── robot_plus_tool.png ├── fcpp_robot_0_5m_plus_tool_0_2m.png ├── fcpp_robot_0_5m_plus_tool_0_5m.png └── robot_plus_tool.ipe ├── maps ├── grid.png ├── basement.png ├── grids │ ├── grid.png │ ├── grid_1.png │ ├── grid_2.png │ ├── grid_3.png │ ├── grid_4.png │ ├── grid_5.png │ ├── grid_6.png │ ├── grid_7.png │ ├── grid_8.png │ ├── grid_9.png │ ├── grid_1_startend.png │ ├── grid_2_startend.png │ ├── grid_3_startend.png │ ├── grid_4_startend.png │ ├── grid_5_startend.png │ ├── grid_6_startend.png │ ├── grid_7_startend.png │ ├── grid_8_startend.png │ ├── grid_9_startend.png │ ├── grid_1.yaml │ ├── grid_2.yaml │ ├── grid_3.yaml │ ├── grid_4.yaml │ ├── grid_5.yaml │ ├── grid_6.yaml │ ├── grid_7.yaml │ ├── grid_8.yaml │ └── grid_9.yaml ├── small_grid.png ├── full_148a_map.pgm ├── no_obstacles_grid.png ├── one_obstacle_grid.png ├── grid_boustro_example.png ├── small_grid_one_obstacle.png ├── grid.yaml ├── small_grid.yaml ├── no_obstacles_grid.yaml ├── one_obstacle_grid.yaml ├── basement.yaml ├── small_grid_one_obstacle.yaml └── full_148a_map.yaml ├── test ├── full_coverage_path_planner │ ├── param │ │ ├── zigzag_planner.yaml │ │ ├── spiral_planner.yaml │ │ ├── aco_planner.yaml │ │ ├── controllers.yaml │ │ └── local_costmap_params.yaml │ ├── test_zigzag_fcpp.launch │ ├── test_spiral_fcpp.launch │ ├── test_aco_fcpp.launch │ ├── fcpp.rviz │ └── rviz_aco.rviz ├── simple_goal.yaml ├── README.md ├── include │ └── full_coverage_path_planner │ │ └── util.h └── src │ ├── util.cpp │ ├── test_common.cpp │ └── test_spiral_stc.cpp ├── fcpp_plugin.xml ├── .vscode └── settings.json ├── package.xml ├── include └── full_coverage_path_planner │ ├── aco.h │ ├── spiral_stc.h │ ├── uturn.h │ ├── ant.h │ ├── common.h │ └── full_coverage_path_planner.h ├── CMakeLists.txt ├── scripts └── adjacency_graph_grid.py ├── nodes └── coverage_progress ├── README.md └── LICENSE /src/full_coverage_path_planner/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /doc/bsa.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/doc/bsa.png -------------------------------------------------------------------------------- /maps/grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grid.png -------------------------------------------------------------------------------- /maps/basement.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/basement.png -------------------------------------------------------------------------------- /maps/grids/grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid.png -------------------------------------------------------------------------------- /maps/small_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/small_grid.png -------------------------------------------------------------------------------- /doc/robot_plus_tool.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/doc/robot_plus_tool.png -------------------------------------------------------------------------------- /maps/full_148a_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/full_148a_map.pgm -------------------------------------------------------------------------------- /maps/grids/grid_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_1.png -------------------------------------------------------------------------------- /maps/grids/grid_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_2.png -------------------------------------------------------------------------------- /maps/grids/grid_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_3.png -------------------------------------------------------------------------------- /maps/grids/grid_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_4.png -------------------------------------------------------------------------------- /maps/grids/grid_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_5.png -------------------------------------------------------------------------------- /maps/grids/grid_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_6.png -------------------------------------------------------------------------------- /maps/grids/grid_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_7.png -------------------------------------------------------------------------------- /maps/grids/grid_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_8.png -------------------------------------------------------------------------------- /maps/grids/grid_9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_9.png -------------------------------------------------------------------------------- /maps/no_obstacles_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/no_obstacles_grid.png -------------------------------------------------------------------------------- /maps/one_obstacle_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/one_obstacle_grid.png -------------------------------------------------------------------------------- /maps/grid_boustro_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grid_boustro_example.png -------------------------------------------------------------------------------- /maps/grids/grid_1_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_1_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_2_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_2_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_3_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_3_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_4_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_4_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_5_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_5_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_6_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_6_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_7_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_7_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_8_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_8_startend.png -------------------------------------------------------------------------------- /maps/grids/grid_9_startend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/grids/grid_9_startend.png -------------------------------------------------------------------------------- /maps/small_grid_one_obstacle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/maps/small_grid_one_obstacle.png -------------------------------------------------------------------------------- /doc/fcpp_robot_0_5m_plus_tool_0_2m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/doc/fcpp_robot_0_5m_plus_tool_0_2m.png -------------------------------------------------------------------------------- /doc/fcpp_robot_0_5m_plus_tool_0_5m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jynxmagic/FCPP-Survey/HEAD/doc/fcpp_robot_0_5m_plus_tool_0_5m.png -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/zigzag_planner.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: 'UTurn' 3 | type: 'full_coverage_path_planner/UTurn' -------------------------------------------------------------------------------- /maps/grid.yaml: -------------------------------------------------------------------------------- 1 | image: grid.png 2 | resolution: 0.05 3 | origin: [-5, -5, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/spiral_planner.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: 'SpiralSTC' 3 | type: 'full_coverage_path_planner/SpiralSTC' -------------------------------------------------------------------------------- /maps/grids/grid_1.yaml: -------------------------------------------------------------------------------- 1 | image: grid_1.png 2 | resolution: 0.05 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_2.yaml: -------------------------------------------------------------------------------- 1 | image: grid_2.png 2 | resolution: 0.05 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_3.yaml: -------------------------------------------------------------------------------- 1 | image: grid_3.png 2 | resolution: 0.05 3 | origin: [-1, -2, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_4.yaml: -------------------------------------------------------------------------------- 1 | image: grid_4.png 2 | resolution: 0.05 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_5.yaml: -------------------------------------------------------------------------------- 1 | image: grid_5.png 2 | resolution: 0.06 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_6.yaml: -------------------------------------------------------------------------------- 1 | image: grid_6.png 2 | resolution: 0.05 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_7.yaml: -------------------------------------------------------------------------------- 1 | image: grid_7.png 2 | resolution: 0.05 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_8.yaml: -------------------------------------------------------------------------------- 1 | image: grid_8.png 2 | resolution: 0.05 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/grids/grid_9.yaml: -------------------------------------------------------------------------------- 1 | image: grid_9.png 2 | resolution: 0.05 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/small_grid.yaml: -------------------------------------------------------------------------------- 1 | image: small_grid.png 2 | resolution: 0.1 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/aco_planner.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: 'AntColonyOptimization' 3 | type: 'full_coverage_path_planner/AntColonyOptimization' -------------------------------------------------------------------------------- /maps/no_obstacles_grid.yaml: -------------------------------------------------------------------------------- 1 | image: no_obstacles_grid.png 2 | resolution: 0.1 3 | origin: [-1, -10, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/one_obstacle_grid.yaml: -------------------------------------------------------------------------------- 1 | image: one_obstacle_grid.png 2 | resolution: 0.1 3 | origin: [-1, -10, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/basement.yaml: -------------------------------------------------------------------------------- 1 | image: basement.png 2 | resolution: 0.050000 3 | origin: [-24.024998, -6.275000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/small_grid_one_obstacle.yaml: -------------------------------------------------------------------------------- 1 | image: small_grid_one_obstacle.png 2 | resolution: 0.1 3 | origin: [-1, -1, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/full_148a_map.yaml: -------------------------------------------------------------------------------- 1 | image: ./full_148a_map.pgm 2 | resolution: 0.100000 3 | origin: [-20.000000, -20.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/controllers.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | - name: 'tracking_pid' 3 | type: 'tracking_pid/TrackingPidLocalPlanner' 4 | 5 | controller: 6 | holonomic_robot: false 7 | 8 | mbf_tolerance_check: true 9 | dist_tolerance: 0.2 10 | angle_tolerance: 0.5 11 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_link 4 | update_frequency: 10.0 5 | publish_frequency: 5.0 6 | width: 5.0 7 | height: 5.0 8 | resolution: 0.05 9 | static_map: false 10 | rolling_window: true 11 | -------------------------------------------------------------------------------- /test/simple_goal.yaml: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 1 3 | stamp: 4 | secs: 0 5 | nsecs: 0 6 | frame_id: 'map' 7 | goal_id: 8 | stamp: 9 | secs: 0 10 | nsecs: 0 11 | id: '' 12 | goal: 13 | target_pose: 14 | header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: 'map' 20 | pose: 21 | position: 22 | x: 0.0 23 | y: 0.0 24 | z: 0.0 25 | orientation: 26 | x: 0.0 27 | y: 0.0 28 | z: 0.0 29 | w: 1.0 30 | -------------------------------------------------------------------------------- /test/README.md: -------------------------------------------------------------------------------- 1 | Test for Full coverage path planner 2 | =================================== 3 | 4 | The full coverage path planner consists of several parts that are each tested separately. 5 | 6 | The move_base_flex plugin consists of several parts, each unit-tested separately: 7 | - test_common: tests common.h 8 | - test_spiral_stc: tests static functions of spiral_stc.h 9 | 10 | Besides unittests, there are also some launch files that both illustrate how to use the 11 | - SpiralSTC-plugin, in test/full_coverage_path_planner/test_full_coverage_path_planner.launch 12 | 13 | Note that the .launch-files do not do any automatic testing or verification of anything, 14 | they are there to make manual testing easier. 15 | -------------------------------------------------------------------------------- /fcpp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Plans a path that covers all accessible points in a costmap by using Spiral-STC. 5 | Spiral-STC works by following the walls and spiraling inwards until it cannot go further. 6 | Then it uses A* to go back outside of the current spiral and then spirals again. 7 | 8 | 9 | 10 | 11 | Plans a path thats covers all accessible points in a costmap using a simple straight line pattern. 12 | 13 | 14 | 15 | 16 | Plans a full coverage path using the ant colony optimization method. 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "algorithm": "cpp", 4 | "cmath": "cpp", 5 | "cstddef": "cpp", 6 | "cstdint": "cpp", 7 | "cstdio": "cpp", 8 | "cstdlib": "cpp", 9 | "cstring": "cpp", 10 | "cwchar": "cpp", 11 | "exception": "cpp", 12 | "fstream": "cpp", 13 | "initializer_list": "cpp", 14 | "ios": "cpp", 15 | "iosfwd": "cpp", 16 | "iostream": "cpp", 17 | "istream": "cpp", 18 | "limits": "cpp", 19 | "list": "cpp", 20 | "memory": "cpp", 21 | "new": "cpp", 22 | "ostream": "cpp", 23 | "set": "cpp", 24 | "stdexcept": "cpp", 25 | "streambuf": "cpp", 26 | "string": "cpp", 27 | "system_error": "cpp", 28 | "tuple": "cpp", 29 | "type_traits": "cpp", 30 | "typeinfo": "cpp", 31 | "utility": "cpp", 32 | "vector": "cpp", 33 | "xfacet": "cpp", 34 | "xiosbase": "cpp", 35 | "xlocale": "cpp", 36 | "xlocinfo": "cpp", 37 | "xlocnum": "cpp", 38 | "xmemory": "cpp", 39 | "xmemory0": "cpp", 40 | "xstddef": "cpp", 41 | "xstring": "cpp", 42 | "xtr1common": "cpp", 43 | "xtree": "cpp", 44 | "xutility": "cpp" 45 | } 46 | } -------------------------------------------------------------------------------- /test/include/full_coverage_path_planner/util.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 27-9-18. 5 | // 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #ifndef FULL_COVERAGE_PATH_PLANNER_UTIL_H 13 | #define FULL_COVERAGE_PATH_PLANNER_UTIL_H 14 | /** 15 | * Create a X*Y grid 16 | * 17 | * @param x number of elements in horizontal direction (columns) 18 | * @param y number of elements in vertical direction (rows) 19 | * @param fill what to fill the rows with? 20 | * @return a vector of vectors. The inner vector has size x, the outer vector contains y x-sized vectors 21 | */ 22 | std::vector > makeTestGrid(int x, int y, bool fill = false); 23 | /** 24 | * Fill a test grid with a fraction of random obstacles 25 | * @param grid a vector of vectors that will be modified to have obstacles (true elements) in random places 26 | * @param obstacle_fraction between 0 and 100, what is the percentage of cells that must be marked as obstacle 27 | * @return bool indicating success 28 | */ 29 | bool randomFillTestGrid(std::vector > &grid, float obstacle_fraction); 30 | 31 | bool operator==(const Point_t &lhs, const Point_t &rhs); 32 | 33 | struct CompareByPosition 34 | { 35 | bool operator()(const Point_t &lhs, const Point_t &rhs) 36 | { 37 | if (lhs.x != rhs.x) 38 | { 39 | return lhs.x < rhs.x; 40 | } 41 | return lhs.y < rhs.y; 42 | } 43 | }; 44 | 45 | #endif // FULL_COVERAGE_PATH_PLANNER_UTIL_H 46 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | full_coverage_path_planner 4 | 0.6.4 5 | Full coverage path planning provides a move_base_flex plugin that can plan a path that will fully cover a given area 6 | Cesar Lopez 7 | Ferry Schoenmakers 8 | Tim Clephas 9 | Jerrel Unkel 10 | Loy van Beek 11 | Yury Brodskiy 12 | Cesar Lopez 13 | 14 | Apache 2.0 15 | http://wiki.ros.org/full_coverage_path_planner 16 | 17 | catkin 18 | roslint 19 | rostest 20 | base_local_planner 21 | costmap_2d 22 | pluginlib 23 | nav_core 24 | roscpp 25 | tf 26 | amcl 27 | joint_state_publisher 28 | map_server 29 | move_base 30 | move_base_flex 31 | cv_bridge 32 | rosunit 33 | tracking_pid 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /test/src/util.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 27-9-18. 5 | // 6 | 7 | #include 8 | #include 9 | 10 | std::vector > makeTestGrid(int x, int y, bool fill) 11 | { 12 | std::vector > grid; 13 | for (int i = 0; i < y; ++i) 14 | { 15 | std::vector row; 16 | for (int j = 0; j < x; ++j) 17 | { 18 | row.push_back(fill); 19 | } 20 | grid.push_back(row); 21 | } 22 | return grid; 23 | } 24 | 25 | bool operator==(const Point_t &lhs, const Point_t &rhs) 26 | { 27 | return lhs.x == rhs.x && lhs.y == rhs.y; 28 | } 29 | 30 | bool randomFillTestGrid(std::vector > &grid, float obstacle_fraction) 31 | { 32 | unsigned int seed = time(NULL); 33 | int max_y = grid.size(); 34 | if (max_y < 1) 35 | { 36 | // Cannot work on less than a row 37 | return false; 38 | } 39 | int max_x = grid.front().size(); 40 | if (max_x < 1) 41 | { 42 | // Cannot work on less than a column 43 | return false; 44 | } 45 | 46 | int total_cells = max_y * max_x; 47 | int total_obstacles = total_cells * (obstacle_fraction / 100); 48 | // std::cout << "total_cells: " << total_cells << ", total_obstacles: " << total_obstacles << std::endl; 49 | 50 | // For the amount of obstacles we need to create, generate random coordinates and insert an obstacle 51 | for (int i = 0; i < total_obstacles; ++i) 52 | { 53 | int x = rand_r(&seed) % max_x; 54 | int y = rand_r(&seed) % max_y; 55 | // std::cout << "Obstacle at (" << x << ", " << y << ")" << std::endl; 56 | grid[y][x] = true; 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/aco.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2022] Manchester Metropolitian University" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using std::string; 22 | 23 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 24 | #include "full_coverage_path_planner/ant.h" 25 | 26 | namespace full_coverage_path_planner 27 | { 28 | class AntColonyOptimization: public nav_core::BaseGlobalPlanner, private full_coverage_path_planner::FullCoveragePathPlanner 29 | { 30 | public: 31 | std::list runACO(std::vector > const &grid, 32 | Point_t &init); 33 | private: 34 | bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 35 | std::vector &plan); 36 | /** 37 | * @brief Initialization function for the FullCoveragePathPlanner object 38 | * @param name The name of this planner 39 | * @param costmap A pointer to the ROS wrapper of the costmap to use for planning 40 | */ 41 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 42 | }; 43 | }; -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(full_coverage_path_planner) 3 | add_compile_options(-std=c++11) 4 | 5 | find_package(catkin REQUIRED 6 | COMPONENTS 7 | base_local_planner 8 | costmap_2d 9 | nav_core 10 | pluginlib 11 | roscpp 12 | roslint 13 | rostest 14 | tf 15 | ) 16 | 17 | include_directories( 18 | include 19 | test/include 20 | ${catkin_INCLUDE_DIRS} 21 | ) 22 | add_definitions(${EIGEN3_DEFINITIONS}) 23 | 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES ${PROJECT_NAME} 27 | CATKIN_DEPENDS 28 | base_local_planner 29 | costmap_2d 30 | nav_core 31 | pluginlib 32 | roscpp 33 | ) 34 | 35 | add_library(${PROJECT_NAME} 36 | src/common.cpp 37 | src/${PROJECT_NAME}.cpp 38 | src/spiral_stc.cpp 39 | src/uturn.cpp 40 | src/ant.cpp 41 | src/aco.cpp 42 | ) 43 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 44 | target_link_libraries(${PROJECT_NAME} 45 | ${catkin_LIBRARIES} 46 | ) 47 | 48 | install(TARGETS 49 | ${PROJECT_NAME} 50 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | ) 53 | 54 | install(DIRECTORY include/${PROJECT_NAME} 55 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 56 | ) 57 | 58 | install(FILES fcpp_plugin.xml 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 60 | ) 61 | 62 | catkin_install_python( 63 | PROGRAMS 64 | nodes/coverage_progress 65 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | ) 67 | 68 | roslint_cpp() 69 | -------------------------------------------------------------------------------- /src/full_coverage_path_planner/stroke_joins.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from math import sqrt 3 | 4 | def half_circle(start, end, previous=None): 5 | """Generate a half circle between start and end, returns interpolated vector i and half-circle j coordinates""" 6 | 7 | r = (end - start) / 2 8 | for i in np.linspace(start, end)[1:-1]: 9 | # circle formula: j = sqrt(r^2 - (i-a)^2) + b 10 | # a is offset in i and in our case identical to the radius 11 | # b is offset in j and not relevant in our case 12 | j = sqrt(r ** 2 - (i - start - r) ** 2) 13 | yield (i, j) 14 | 15 | 16 | def mwm(start, end, previous, v_depth=1.0, v_bottom_off_center=0.0): 17 | """Generate a path that connects two strokes via a V, so that the two strokes combined look like an M, 18 | with a small v in the middle 19 | :param start: one top of the v 20 | :param end: the other top of the v 21 | :param previous: To indicate direction of the V (to not make a ^), previous is the point that comes before the start of the V (so the bottom-left point of an M or top-left in a W) 22 | :return: yields all the points of the v (so the bottom of the v) 23 | 24 | >>> mwm(np.array((1, 2)), np.array((2, 3)), np.array((3, 0))) 25 | array([ 2., 2.]) 26 | """ 27 | 28 | # This needs to know in which direction to put the V. This may or may not be alternating even: 29 | # The pattern could be M M M but just as well M M M 30 | # W W (v point up) V V with the inner-v point 31 | start, end, previous = np.array(start), np.array(end), np.array(previous) 32 | 33 | r = (end - start) / 2. 34 | middle = start + r 35 | 36 | delta_ver = previous - start 37 | delta_hor = start - end 38 | 39 | distance_hor = np.linalg.norm(delta_hor) 40 | distance_ver = np.linalg.norm(delta_ver) 41 | 42 | normalized_delta_hor = (delta_hor / distance_hor) if np.any(delta_hor) else 0.0 43 | normalized_delta_ver = (delta_ver / distance_ver) if np.any(delta_ver) else 0.0 44 | 45 | # try: 46 | v = middle + (normalized_delta_ver * distance_hor * v_depth) 47 | v2 = v + (normalized_delta_hor * v_bottom_off_center * r) 48 | 49 | yield tuple(v2) -------------------------------------------------------------------------------- /include/full_coverage_path_planner/spiral_stc.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using std::string; 22 | 23 | #ifndef FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 24 | #define FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 25 | 26 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 27 | namespace full_coverage_path_planner 28 | { 29 | class SpiralSTC : public nav_core::BaseGlobalPlanner, private full_coverage_path_planner::FullCoveragePathPlanner 30 | { 31 | public: 32 | /** 33 | * Find a path that spirals inwards from init until an obstacle is seen in the grid 34 | * @param grid 2D grid of bools. true == occupied/blocked/obstacle 35 | * @param init start position 36 | * @param visited all the nodes visited by the spiral 37 | * @return list of nodes that form the spiral 38 | */ 39 | static std::list spiral(std::vector > const &grid, std::list &init, 40 | std::vector > &visited); 41 | 42 | /** 43 | * Perform Spiral-STC (Spanning Tree Coverage) coverage path planning. 44 | * In essence, the robot moves forward until an obstacle or visited node is met, then turns right (making a spiral) 45 | * 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 46 | * @param grid 47 | * @param init 48 | * @return 49 | */ 50 | static std::list spiral_stc(std::vector > const &grid, 51 | Point_t &init, 52 | int &multiple_pass_counter, 53 | int &visited_counter); 54 | 55 | private: 56 | /** 57 | * @brief Given a goal pose in the world, compute a plan 58 | * @param start The start pose 59 | * @param goal The goal pose 60 | * @param plan The plan... filled by the planner 61 | * @return True if a valid plan was found, false otherwise 62 | */ 63 | bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 64 | std::vector &plan); 65 | 66 | /** 67 | * @brief Initialization function for the FullCoveragePathPlanner object 68 | * @param name The name of this planner 69 | * @param costmap A pointer to the ROS wrapper of the costmap to use for planning 70 | */ 71 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 72 | }; 73 | 74 | } // namespace full_coverage_path_planner 75 | #endif // FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 76 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/uturn.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2022] Manchester Metropolitian University" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using std::string; 22 | 23 | #ifndef FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 24 | #define FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 25 | 26 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 27 | namespace full_coverage_path_planner 28 | { 29 | class UTurn : public nav_core::BaseGlobalPlanner, private full_coverage_path_planner::FullCoveragePathPlanner 30 | { 31 | public: 32 | /** 33 | * Find a path that spirals inwards from init until an obstacle is seen in the grid 34 | * @param grid 2D grid of bools. true == occupied/blocked/obstacle 35 | * @param init start position 36 | * @param visited all the nodes visited by the spiral 37 | * @return list of nodes that form the spiral 38 | */ 39 | static std::list move(std::vector > const &grid, std::list &init, 40 | std::vector > &visited); 41 | 42 | /** 43 | * Perform Spiral-STC (Spanning Tree Coverage) coverage path planning. 44 | * In essence, the robot moves forward until an obstacle or visited node is met, then turns right (making a spiral) 45 | * 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 46 | * @param grid 47 | * @param init 48 | * @return 49 | */ 50 | static std::list moveFull(std::vector > const &grid, 51 | Point_t &init, 52 | int &multiple_pass_counter, 53 | int &visited_counter); 54 | 55 | private: 56 | /** 57 | * @brief Given a goal pose in the world, compute a plan 58 | * @param start The start pose 59 | * @param goal The goal pose 60 | * @param plan The plan... filled by the planner 61 | * @return True if a valid plan was found, false otherwise 62 | */ 63 | bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 64 | std::vector &plan); 65 | 66 | /** 67 | * @brief Initialization function for the FullCoveragePathPlanner object 68 | * @param name The name of this planner 69 | * @param costmap A pointer to the ROS wrapper of the costmap to use for planning 70 | */ 71 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 72 | }; 73 | 74 | } // namespace full_coverage_path_planner 75 | #endif // FULL_COVERAGE_PATH_PLANNER_SPIRAL_STC_H 76 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/ant.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2022] Manchester Metropolitian University" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using std::string; 21 | 22 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 23 | 24 | namespace full_coverage_path_planner 25 | { 26 | class Ant 27 | { 28 | public: 29 | 30 | std::vector> personal_pheromone_grid; 31 | std::vector> personal_probability_matrix; 32 | float current_path_score; 33 | int visited_counter; 34 | int multiple_pass_counter; 35 | int coverage; 36 | int ant_velocity; 37 | Direction direction; 38 | std::list current_path; 39 | gridNode_t current_location; 40 | float pheromone_rate; 41 | 42 | /** 43 | * 44 | * */ 45 | Ant(gridNode_t start_point, std::vector> pheromone_grid, std::vector> score_grid, int velocity); 46 | /** 47 | * 48 | **/ 49 | _Float64 producePheromones(_Float64 evaporation_rate, _Float64 pheromone_rate, _Float64 system_constant); 50 | /** 51 | * @brief 52 | * 53 | */ 54 | void calculateProbabilityMatrix(std::vector> global_pheromone_grid); 55 | /** 56 | * @brief 57 | * 58 | */ 59 | std::list getPossibleMovements(std::vector> visited, int loc_x, int loc_y); 60 | 61 | /** 62 | * @brief 63 | * 64 | */ 65 | bool resolveDeadlock(std::vector> visited, std::list goals, std::vector> const& grid); 66 | /** 67 | * 68 | * */ 69 | bool canMove(std::vector> const& grid, 70 | std::list to); 71 | /** 72 | * 73 | * */ 74 | void move( 75 | gridNode_t new_location, 76 | bool add_to_path 77 | ); 78 | /** 79 | * @brief 80 | * 81 | */ 82 | gridNode_t getBestMovement(std::list possible_movements, std::vector> global_pheromone_grid); 83 | 84 | 85 | /** 86 | * @brief 87 | * 88 | */ 89 | std::list generateProbabilityDistro(std::list possible_movements, std::vector> global_pheromone_grid); 90 | }; 91 | }; -------------------------------------------------------------------------------- /scripts/adjacency_graph_grid.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | from copy import deepcopy 4 | from typing import List 5 | 6 | class Node: 7 | def __init__(self, ID) -> None: 8 | self.adjacent = [] 9 | self.ID = ID 10 | 11 | def add_adjacent(self, node) -> None: 12 | self.adjacent.append(node) 13 | 14 | def get_adjacent(self): 15 | return self.adjacent 16 | 17 | 18 | def get_ID(self) -> int: 19 | return self.ID 20 | 21 | class Graph: 22 | def __init__(self) -> None: 23 | self.nodes = [] 24 | 25 | def add_node(self, node) -> None: 26 | self.nodes.append(node) 27 | 28 | def get_node_by_ID(self, id) -> Node: 29 | for n in self.nodes: 30 | if n.get_ID() == id: 31 | return n 32 | 33 | 34 | def generate_graph() -> Graph: 35 | g = Graph() 36 | 37 | grid_1 = Node(1) 38 | grid_2 = Node(2) 39 | grid_3 = Node(3) 40 | grid_4 = Node(4) 41 | grid_5 = Node(5) 42 | grid_6 = Node(6) 43 | grid_7 = Node(7) 44 | grid_8 = Node(8) 45 | grid_9 = Node(9) 46 | 47 | grid_1.add_adjacent(grid_2) 48 | grid_1.add_adjacent(grid_4) 49 | 50 | grid_2.add_adjacent(grid_5) 51 | grid_2.add_adjacent(grid_7) 52 | grid_2.add_adjacent(grid_1) 53 | 54 | grid_3.add_adjacent(grid_4) 55 | 56 | grid_4.add_adjacent(grid_3) 57 | grid_4.add_adjacent(grid_1) 58 | 59 | grid_5.add_adjacent(grid_6) 60 | grid_5.add_adjacent(grid_7) 61 | 62 | grid_6.add_adjacent(grid_5) 63 | grid_6.add_adjacent(grid_9) 64 | 65 | grid_7.add_adjacent(grid_8) 66 | grid_7.add_adjacent(grid_2) 67 | 68 | grid_8.add_adjacent(grid_7) 69 | 70 | grid_9.add_adjacent(grid_6) 71 | 72 | g.add_node(grid_1) 73 | g.add_node(grid_2) 74 | g.add_node(grid_3) 75 | g.add_node(grid_4) 76 | g.add_node(grid_5) 77 | g.add_node(grid_6) 78 | g.add_node(grid_7) 79 | g.add_node(grid_8) 80 | g.add_node(grid_9) 81 | 82 | return g 83 | 84 | def is_complete(path_of_nodes) -> bool: 85 | 86 | grid = { 87 | 1 : False, 88 | 2 : False, 89 | 3 : False, 90 | 4 : False, 91 | 5 : False, 92 | 6 : False, 93 | 7 : False, 94 | 8 : False, 95 | 9 : False 96 | } 97 | 98 | for node in path_of_nodes: 99 | grid[node.get_ID()] = True 100 | 101 | complete = True 102 | 103 | for i, _ in enumerate(grid): 104 | if grid[i+1] == False: 105 | complete = False 106 | 107 | return complete 108 | 109 | def bfs_tsp(graph) -> List[Node]: 110 | starting_node = graph.get_node_by_ID(1) 111 | starting_point = [starting_node, [starting_node]] 112 | 113 | open_set = [] 114 | 115 | open_set.append(starting_point) 116 | 117 | while len(open_set) != 0: 118 | node, path = open_set.pop(0) 119 | 120 | print(node.get_ID()) 121 | print(len(path)) 122 | 123 | adjacent_nodes = node.get_adjacent() 124 | for adj in adjacent_nodes: 125 | new_path = deepcopy(path) 126 | new_path.append(adj) 127 | open_set.append([adj, new_path]) 128 | 129 | if(is_complete(path)): 130 | return path 131 | 132 | print("failed to find path") 133 | 134 | 135 | 136 | 137 | if __name__ == "__main__": 138 | adjacency_graph = generate_graph() 139 | shortest_tour = bfs_tsp(adjacency_graph) 140 | 141 | i = 0 142 | for node in shortest_tour: 143 | print(i, node.get_ID()) 144 | i += 1 145 | 146 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/common.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 6-9-18. 5 | // 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #ifndef FULL_COVERAGE_PATH_PLANNER_COMMON_H 12 | #define FULL_COVERAGE_PATH_PLANNER_COMMON_H 13 | 14 | typedef struct 15 | { 16 | int x, y; 17 | } 18 | Point_t; 19 | 20 | inline std::ostream &operator << (std::ostream &os, Point_t &p) 21 | { 22 | return os << "(" << p.x << ", " << p.y << ")"; 23 | } 24 | 25 | typedef struct 26 | { 27 | Point_t pos; 28 | 29 | /** Path cost 30 | * cost of the path from the start node to gridNode_t 31 | */ 32 | int cost; 33 | 34 | /** Heuristic cost 35 | * cost of the cheapest path from this gridNode_t to the goal 36 | */ 37 | int he; 38 | } 39 | gridNode_t; 40 | 41 | enum Direction {NORTH, EAST, SOUTH, WEST}; 42 | 43 | typedef struct 44 | { 45 | gridNode_t pos; 46 | _Float64 proba; 47 | } 48 | Pos_proba; 49 | 50 | typedef struct 51 | { 52 | gridNode_t pos; 53 | _Float64 phera; 54 | } 55 | Pos_phera; 56 | 57 | 58 | inline std::ostream &operator << (std::ostream &os, gridNode_t &g) 59 | { 60 | return os << "gridNode_t(" << g.cost << ", " << g.he << ", " << g.pos << ")"; 61 | } 62 | 63 | 64 | typedef struct 65 | { 66 | float x, y; 67 | } 68 | fPoint_t; 69 | 70 | inline std::ostream &operator << (std::ostream &os, fPoint_t &p) 71 | { 72 | return os << "(" << p.x << ", " << p.y << ")"; 73 | } 74 | 75 | enum 76 | { 77 | eNodeOpen = false, 78 | eNodeVisited = true 79 | }; 80 | 81 | 82 | _Float64 score(_Float64 coverage, int accessable_tiles); 83 | 84 | /** 85 | * Find the distance from poi to the closest point in goals 86 | * @param poi Starting point 87 | * @param goals Potential next points to find the closest of 88 | * @return Distance to the closest point (out of 'goals') to 'poi' 89 | */ 90 | int distanceToClosestPoint(Point_t poi, std::list const &goals); 91 | 92 | /** 93 | * Calculate the distance between two points, squared 94 | */ 95 | int distanceSquared(const Point_t &p1, const Point_t &p2); 96 | 97 | /** 98 | * Perform A* shorted path finding from init to one of the points in heuristic_goals 99 | * @param grid 2D grid of bools. true == occupied/blocked/obstacle 100 | * @param init start position 101 | * @param cost cost of traversing a free node 102 | * @param visited grid 2D grid of bools. true == visited 103 | * @param open_space Open space that A* need to find a path towards. Only used for the heuristic and directing search 104 | * @param pathNodes nodes that form the path from init to the closest point in heuristic_goals 105 | * @return whether we resign from finding a path or not. true is we resign and false if we found a path 106 | */ 107 | bool a_star_to_open_space(std::vector > const &grid, gridNode_t init, int cost, 108 | std::vector > &visited, std::list const &open_space, 109 | std::list &pathNodes); 110 | 111 | /** 112 | * Print a grid according to the internal representation 113 | * @param grid 114 | * @param visited 115 | * @param fullPath 116 | */ 117 | void printGrid(std::vector > const& grid, 118 | std::vector > const& visited, 119 | std::list const& path); 120 | 121 | /** 122 | * Print a grid according to the internal representation 123 | * @param grid 124 | * @param visited 125 | * @param fullPath 126 | * @param start 127 | * @param end 128 | */ 129 | void printGrid(std::vector > const& grid, 130 | std::vector > const& visited, 131 | std::list const& path, 132 | gridNode_t start, 133 | gridNode_t end); 134 | 135 | /** 136 | * Print a 2D array of bools to stdout 137 | */ 138 | void printGrid(std::vector > const& grid); 139 | 140 | 141 | void printGrid(std::vector > const& grid); 142 | 143 | int manhattenDistance(const Point_t& p1, const Point_t& p2); 144 | 145 | 146 | /** 147 | * Convert 2D grid of bools to a list of Point_t 148 | * @param grid 2D grid representing a map 149 | * @param value_to_search points matching this value will be returned 150 | * @return a list of points that have the given value_to_search 151 | */ 152 | std::list map_2_goals(std::vector > const& grid, bool value_to_search); 153 | #endif // FULL_COVERAGE_PATH_PLANNER_COMMON_H 154 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/full_coverage_path_planner.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | /** include the libraries you need in your planner here */ 5 | /** for global path planner interface */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using std::string; 25 | 26 | #ifndef FULL_COVERAGE_PATH_PLANNER_FULL_COVERAGE_PATH_PLANNER_H 27 | #define FULL_COVERAGE_PATH_PLANNER_FULL_COVERAGE_PATH_PLANNER_H 28 | 29 | #include "full_coverage_path_planner/common.h" 30 | 31 | // #define DEBUG_PLOT 32 | 33 | #ifndef dabs 34 | #define dabs(a) ((a) >= 0 ? (a):-(a)) 35 | #endif 36 | #ifndef dmin 37 | #define dmin(a, b) ((a) <= (b) ? (a):(b)) 38 | #endif 39 | #ifndef dmax 40 | #define dmax(a, b) ((a) >= (b) ? (a):(b)) 41 | #endif 42 | #ifndef clamp 43 | #define clamp(a, lower, upper) dmax(dmin(a, upper), lower) 44 | #endif 45 | 46 | enum 47 | { 48 | eDirNone = 0, 49 | eDirRight = 1, 50 | eDirUp = 2, 51 | eDirLeft = -1, 52 | eDirDown = -2, 53 | }; 54 | 55 | namespace full_coverage_path_planner 56 | { 57 | class FullCoveragePathPlanner 58 | { 59 | public: 60 | /** 61 | * @brief Default constructor for the NavFnROS object 62 | */ 63 | FullCoveragePathPlanner(); 64 | FullCoveragePathPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 65 | 66 | /** 67 | * @brief Publish a path for visualization purposes 68 | */ 69 | void publishPlan(const std::vector& path); 70 | 71 | ~FullCoveragePathPlanner() 72 | { 73 | } 74 | 75 | virtual bool makePlan(const geometry_msgs::PoseStamped& start, 76 | const geometry_msgs::PoseStamped& goal, std::vector& plan) = 0; 77 | 78 | protected: 79 | /** 80 | * Convert internal representation of a to a ROS path 81 | * @param start Start pose of robot 82 | * @param goalpoints Goal points from Spiral Algorithm 83 | * @param plan Output plan variable 84 | */ 85 | void parsePointlist2Plan(const geometry_msgs::PoseStamped& start, std::list const& goalpoints, 86 | std::vector& plan); 87 | 88 | /** 89 | * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile 90 | * @param cpp_grid_ ROS occupancy grid representation. Cells higher that 65 are considered occupied 91 | * @param grid internal map representation 92 | * @param tileSize size (in meters) of a cell. This can be the robot's size 93 | * @param realStart Start position of the robot (in meters) 94 | * @param scaledStart Start position of the robot on the grid 95 | * @return success 96 | */ 97 | bool parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_, 98 | std::vector >& grid, 99 | float robotRadius, 100 | float toolRadius, 101 | geometry_msgs::PoseStamped const& realStart, 102 | Point_t& scaledStart); 103 | ros::Publisher plan_pub_; 104 | ros::ServiceClient cpp_grid_client_; 105 | nav_msgs::OccupancyGrid cpp_grid_; 106 | float robot_radius_; 107 | float tool_radius_; 108 | float plan_resolution_; 109 | float tile_size_; 110 | fPoint_t grid_origin_; 111 | bool initialized_; 112 | geometry_msgs::PoseStamped previous_goal_; 113 | 114 | struct spiral_cpp_metrics_type 115 | { 116 | int visited_counter; 117 | int multiple_pass_counter; 118 | int accessible_counter; 119 | double total_area_covered; 120 | }; 121 | spiral_cpp_metrics_type spiral_cpp_metrics_; 122 | }; 123 | 124 | 125 | /** 126 | * Sort function for sorting Points on distance to a POI 127 | */ 128 | struct ComparatorForPointSort 129 | { 130 | explicit ComparatorForPointSort(Point_t poi) : _poi(poi) 131 | { 132 | } 133 | 134 | bool operator()(const Point_t& first, const Point_t& second) const 135 | { 136 | return distanceSquared(first, _poi) < distanceSquared(second, _poi); 137 | } 138 | 139 | private: 140 | Point_t _poi; 141 | }; 142 | } // namespace full_coverage_path_planner 143 | #endif // FULL_COVERAGE_PATH_PLANNER_FULL_COVERAGE_PATH_PLANNER_H 144 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_zigzag_fcpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_spiral_fcpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_aco_fcpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /src/ant.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2022] Manchester Metropolitian University" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "full_coverage_path_planner/ant.h" 11 | 12 | namespace full_coverage_path_planner 13 | { 14 | Ant::Ant(gridNode_t start_point, std::vector> pheromone_grid, std::vector> probability_matrix, int velocity) 15 | { 16 | current_location = start_point; 17 | personal_pheromone_grid = pheromone_grid; 18 | personal_probability_matrix = probability_matrix; 19 | ant_velocity = velocity; 20 | } 21 | 22 | _Float64 Ant::producePheromones(_Float64 evaporation_rate, _Float64 pheromone_rate, _Float64 system_constant) 23 | { 24 | _Float64 pheromones = evaporation_rate * (personal_pheromone_grid[current_location.pos.y][current_location.pos.x] + (pheromone_rate * system_constant)); 25 | personal_pheromone_grid[current_location.pos.y][current_location.pos.x] = pheromones; 26 | return pheromones; 27 | } 28 | 29 | gridNode_t Ant::getBestMovement(std::list possible_movements, std::vector> global_pheromone_grid) 30 | { 31 | _Float64 bestScore = -INFINITY; 32 | gridNode_t bestPoint; 33 | 34 | for (gridNode_t point : possible_movements) 35 | { 36 | _Float64 pheramone_at_pos = global_pheromone_grid[point.pos.y][point.pos.x]; 37 | 38 | if (pheramone_at_pos > bestScore) 39 | { 40 | bestScore = pheramone_at_pos; 41 | bestPoint = point; 42 | } 43 | } 44 | 45 | return bestPoint; 46 | } 47 | 48 | std::list Ant::generateProbabilityDistro(std::list possible_movements, std::vector> global_pheromone_grid) 49 | { 50 | std::list pheromonesList; 51 | _Float64 pheramoneSum = 0; 52 | for (gridNode_t point : possible_movements) 53 | { 54 | _Float64 pheramone = personal_probability_matrix[point.pos.y][point.pos.x]; 55 | pheramoneSum += pheramone; 56 | pheromonesList.push_back({point, pheramone}); 57 | } 58 | 59 | std::list probabiltyDistro; 60 | for (Pos_phera pp : pheromonesList) 61 | { 62 | _Float64 probability = pp.phera / pheramoneSum; // probability normalization 63 | probabiltyDistro.push_back({pp.pos, probability}); 64 | } 65 | 66 | return probabiltyDistro; 67 | } 68 | 69 | void Ant::calculateProbabilityMatrix(std::vector> global_pheromone_grid) 70 | { 71 | int i = 0; 72 | for (std::vector<_Float64> y : personal_probability_matrix) 73 | { 74 | int j = 0; 75 | for (_Float64 x : y) 76 | { 77 | personal_probability_matrix[i][j] = personal_pheromone_grid[i][j] / global_pheromone_grid[i][j]; 78 | j++; 79 | } 80 | i++; 81 | } 82 | } 83 | 84 | std::list Ant::getPossibleMovements(std::vector> visited, int loc_x, int loc_y) 85 | { 86 | uint dx, dy, dx_prev, nRows = visited.size(), nCols = visited[0].size(); 87 | 88 | int orig_x = loc_x; 89 | int orig_y = loc_y; 90 | 91 | std::list possible_movements; 92 | 93 | for (int i = 0; i < 4;) 94 | { 95 | int xt = orig_x; 96 | int yt = orig_y; 97 | 98 | if (i == 0) 99 | { 100 | xt = orig_x + 1; 101 | } 102 | if (i == 1) 103 | { 104 | xt = orig_x - 1; 105 | } 106 | if (i == 2) 107 | { 108 | yt = orig_y + 1; 109 | } 110 | if (i == 3) 111 | { 112 | yt = orig_y - 1; 113 | } 114 | 115 | if (xt >= 0 && xt < nCols && yt >= 0 && yt < nRows) // ant within bounds 116 | { 117 | if (visited[yt][xt] == eNodeOpen) // and location is not part of working memory 118 | { 119 | gridNode_t possible_movement = {{xt, yt}, 0, 0}; // it's possible to move to that location 120 | possible_movements.push_back(possible_movement); 121 | } 122 | } 123 | i++; 124 | } 125 | 126 | return possible_movements; 127 | } 128 | 129 | bool Ant::resolveDeadlock(std::vector> visited, std::list goals, std::vector> const &grid) 130 | { 131 | std::list pathNodesForAstar = current_path; 132 | 133 | bool resign = a_star_to_open_space(grid, current_location, 1, visited, goals, pathNodesForAstar); 134 | 135 | if (!resign) 136 | { 137 | current_path = pathNodesForAstar; 138 | } 139 | 140 | // update visited grid from a* 141 | std::list::iterator it; 142 | for (it = current_path.begin(); it != current_path.end(); ++it) 143 | { 144 | visited[it->pos.y][it->pos.x] = eNodeVisited; 145 | } 146 | 147 | current_location = current_path.back(); 148 | 149 | return resign; 150 | } 151 | 152 | bool Ant::canMove(std::vector> const &grid, 153 | std::list to) 154 | { 155 | for (gridNode_t point : to) 156 | { 157 | if (grid[point.pos.y][point.pos.x] == eNodeOpen) 158 | { 159 | return true; 160 | } 161 | } 162 | return false; 163 | } 164 | 165 | void Ant::move( 166 | gridNode_t new_location, 167 | bool add_to_path) 168 | { 169 | int cx = current_location.pos.x; 170 | int cy = current_location.pos.y; 171 | 172 | int nx = new_location.pos.x; 173 | int ny = new_location.pos.y; 174 | 175 | if (cx + 1 == nx) 176 | { 177 | direction = EAST; 178 | } 179 | else if (cx - 1 == nx) 180 | { 181 | direction = WEST; 182 | } 183 | else if (cy + 1 == ny) 184 | { 185 | direction = NORTH; 186 | } 187 | else 188 | { 189 | direction = SOUTH; 190 | } 191 | 192 | current_location = new_location; // gridNode_t 193 | if (add_to_path) 194 | { 195 | current_path.push_back(current_location); 196 | } 197 | } 198 | } -------------------------------------------------------------------------------- /nodes/coverage_progress: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from nav_msgs.msg import OccupancyGrid 6 | from numpy import ones, sum 7 | from std_msgs.msg import Float32, Header 8 | from std_srvs.srv import Trigger 9 | 10 | # Constants for more readable index lookup 11 | X, Y, Z, W = 0, 1, 2, 3 12 | 13 | 14 | class CoverageProgressNode(object): 15 | """The CoverageProgressNode keeps track of coverage progress. 16 | It does this by periodically looking up the position of the coverage disk in an occupancy grid. 17 | Cells within a radius from this position are 'covered' 18 | 19 | Cell values are interpreted in this way: Lower is covered, higher is less covered 20 | - 100: uncovered (initial value) 21 | - < 100: covered 22 | 23 | The node emits a coverage progress, 24 | which is the ratio of cells considered coverage (0.0 is everything uncovered, 1.0 is all covered) 25 | """ 26 | 27 | DIRTY = 100 28 | 29 | def __init__(self): 30 | self.listener = tf.TransformListener() 31 | 32 | x = None # type: float 33 | y = None # type: float 34 | self._coverage_area = None # type: Tuple[float, float] 35 | 36 | self.coverage_resolution = None # type: float # How big is a cell [m] 37 | 38 | # How well covered is a cell after it has been covered for 1 time step 39 | self.coverage_effectivity = None # type: int 40 | 41 | self.map_frame = None # type: str 42 | self.coverage_frame = None # type: str 43 | 44 | self.grid = self._initialize_map() 45 | 46 | self.progress_pub = rospy.Publisher("coverage_progress", Float32, queue_size=1) 47 | self.grid_pub = rospy.Publisher("coverage_grid", OccupancyGrid, queue_size=1) 48 | 49 | self.reset_srv = rospy.Service('reset', Trigger, self.reset) 50 | 51 | self._rate = rospy.get_param("~rate", 10.0) 52 | self._update_timer = rospy.Timer(rospy.Duration(1.0/self._rate), self._update_callback) 53 | 54 | def _initialize_map(self): 55 | # Initialize coverage matrix 56 | 57 | # Define parameters 58 | x = rospy.get_param("~target_area/x", 10.0) # height of the area to cover, in x direction of the map 59 | y = rospy.get_param("~target_area/y", 5.0) # width of the area to cover, in y direction of the map 60 | self._coverage_area = (x, y) 61 | 62 | try: 63 | self.coverage_radius_meters = rospy.get_param("~coverage_radius") 64 | except KeyError: 65 | try: 66 | self.coverage_radius_meters = rospy.get_param("~coverage_width") / 2.0 67 | except KeyError: 68 | rospy.logerr("Specify either coverage_width or coverage_radius") 69 | raise ValueError("Neither ~coverage_radius nor ~coverage_width specified, one of these is required") 70 | 71 | self.coverage_resolution = rospy.get_param("~coverage_resolution", 0.05) # How big is a cell [m] 72 | 73 | # How much covered is a cell after it has been covered for 1 time step 74 | self.coverage_effectivity = rospy.get_param("~coverage_effectivity", 5) 75 | 76 | self.map_frame = rospy.get_param("~map_frame", "map") 77 | self.coverage_frame = rospy.get_param("~coverage_frame", "base_link") 78 | 79 | self.coverage_radius_meters += 2 * self.coverage_resolution # Compensate for discretization 80 | self.coverage_radius_cells = int((self.coverage_radius_meters) / self.coverage_resolution) 81 | 82 | grid = OccupancyGrid() 83 | grid.info.resolution = self.coverage_resolution 84 | 85 | grid.info.width = abs(int(self._coverage_area[X] / self.coverage_resolution)) 86 | grid.info.height = abs(int(self._coverage_area[Y] / self.coverage_resolution)) 87 | 88 | grid.info.origin.position.x = 0 if self._coverage_area[X] > 0 else self._coverage_area[X] 89 | grid.info.origin.position.y = 0 if self._coverage_area[Y] > 0 else self._coverage_area[Y] 90 | grid.info.origin.orientation.w = 1 91 | 92 | # Initialize OccupancyGrid to have all cells DIRTY 93 | grid.data = self.DIRTY * ones(grid.info.width * grid.info.height) 94 | 95 | return grid 96 | 97 | def _update_callback(self, event): 98 | # Get the position of point (0,0,0) the coverage_disk frame wrt. the map frame (which can both be remapped if need be) 99 | 100 | try: 101 | (coveragepos, rot) = self.listener.lookupTransform(self.map_frame, self.coverage_frame, rospy.Time(0)) 102 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 103 | return 104 | 105 | # Element of matrix corresponding to middle of coverage surface 106 | x_point = int((coveragepos[X] - self.grid.info.origin.position.x) / self.coverage_resolution) 107 | y_point = int((coveragepos[Y] - self.grid.info.origin.position.y) / self.coverage_resolution) 108 | 109 | # Initialize message 110 | self.grid.header = Header() 111 | self.grid.header.frame_id = self.map_frame 112 | 113 | # Loop over amount of cells covered in x (j) and y (i) direction 114 | for i in range(2 * self.coverage_radius_cells): 115 | for j in range(2 * self.coverage_radius_cells): 116 | 117 | x_index = j - self.coverage_radius_cells 118 | y_index = i - self.coverage_radius_cells 119 | 120 | array_index = x_point + x_index + self.grid.info.width * (y_point + y_index) 121 | 122 | cell_in_coverage_circle = x_index ** 2 + y_index ** 2 < self.coverage_radius_cells ** 2 123 | 124 | cell_in_grid = 0 <= x_point + x_index < abs(int(self._coverage_area[X] / self.coverage_resolution)) \ 125 | and 0 <= y_point + y_index < abs(int(self._coverage_area[Y] / self.coverage_resolution)) 126 | 127 | if cell_in_coverage_circle and cell_in_grid: 128 | self.grid.data[array_index] = max(0, self.grid.data[array_index] - self.coverage_effectivity) 129 | else: 130 | rospy.logdebug("x_point %i y_point %i, x_meas %f, y_meas %f", x_point, y_point, coveragepos[X], 131 | coveragepos[Y]) 132 | 133 | coverage_progress = float(sum([self.grid.data < self.DIRTY])) / (self.grid.info.width * self.grid.info.height) 134 | 135 | self.progress_pub.publish(coverage_progress) 136 | self.grid_pub.publish(self.grid) 137 | 138 | def finish_callback(self, msg): 139 | 140 | if msg: 141 | coverage_progress = float(sum([self.grid.data < self.DIRTY])) / (self.grid.info.width * self.grid.info.height) 142 | 143 | self.progress_pub.publish(coverage_progress) 144 | 145 | def reset(self, srv_request): 146 | rospy.loginfo("Reset coverage progress and grid") 147 | self.grid = self._initialize_map() 148 | return (True, "Reset coverage progress and grid") 149 | 150 | if __name__ == '__main__': 151 | rospy.init_node('coverage_progress') 152 | try: 153 | node = CoverageProgressNode() 154 | rospy.spin() 155 | except rospy.ROSInterruptException: 156 | pass 157 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Full Coverage Path Planner (FCPP) 2 | 3 | ## Prerequisites 4 | 5 | -ROS Noetic 6 | (https://wiki.ros.org/noetic) 7 | 8 | -Turtlebot 3 (https://emanual.robotis.com/docs/en/platform/turtlebot3/quick-start/) 9 | 10 | ## Installation 11 | ```export TURTLEBOT3_MODEL=burger``` 12 | 13 | ```cd catkin_ws/src``` 14 | 15 | ```git clone https://github.com/jynxmagic/FCPP-Survey.git``` 16 | 17 | ```git clone https://github.com/mrath/mobile_robot_simulator.git``` 18 | 19 | ```git clone https://github.com/nobleo/tracking_pid.git``` 20 | 21 | ```sudo apt install python3-catkin``` 22 | 23 | ```sudo apt install python3-catkin-tools``` 24 | 25 | ```sudo apt install python3-osrf-pycommon``` 26 | 27 | ```cd ..``` 28 | 29 | ```rosdep install --from-paths src --ignore-src -r -y``` 30 | 31 | ```rm -rf build``` 32 | 33 | ```rm -rf devel``` 34 | 35 | ```catkin_make``` 36 | 37 | ```source devel/setup.bash``` 38 | 39 | ## testing 40 | ```roslaunch full_coverage_path_planner test_aco_fcpp.launch``` 41 | 42 | ## results 43 | Simple Grid (maps/grid.yaml): 44 | ![image](https://user-images.githubusercontent.com/38462612/143719248-c0a64a86-4d2b-4c60-9f46-355a469e9c75.png) 45 | Basement (maps/basement.yaml): 46 | ![image](https://user-images.githubusercontent.com/38462612/143719911-b335fa5a-bf64-411b-bcad-2e9fc4470327.png) 47 | 48 | Green lines are places which are to be searched. Blue lines indicate the path the robot has already taken. 49 | 50 | 51 | ----- 52 | ## Forked repo description -> 53 | 54 | ## Overview 55 | 56 | This package provides an implementation of a Full Coverage Path Planner (FCPP) using the Backtracking Spiral Algorithm (BSA), see [1]. 57 | 58 | This packages acts as a global planner plugin to the Move Base package (http://wiki.ros.org/move_base). 59 | 60 | ![BSA](doc/fcpp_robot_0_5m_plus_tool_0_2m.png) 61 | 62 | The user can configure robot radius and tool radius separately: 63 | 64 | ![robot_plus_tool](doc/robot_plus_tool.png) 65 | 66 | 67 | **Keywords:** coverage path planning, move base 68 | 69 | ### License 70 | 71 | Apache 2.0 72 | 73 | **Author(s): Yury Brodskiy, Ferry Schoenmakers, Tim Clephas, Jerrel Unkel, Loy van Beek, Cesar lopez** 74 | 75 | **Maintainer: Cesar Lopez, cesar.lopez@nobleo.nl** 76 | 77 | **Affiliation: Nobleo Projects BV, Eindhoven, the Netherlands** 78 | 79 | The Full Coverage Path Planner package has been tested under [ROS] Melodic and Ubuntu 18.04. 80 | 81 | ## Installation 82 | 83 | 84 | ### Building from Source 85 | 86 | 87 | #### Dependencies 88 | 89 | - [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), 90 | - [Move Base Flex (MBF)](http://wiki.ros.org/move_base_flex) (move base flex node) used for system testing 91 | 92 | 93 | #### Building 94 | 95 | To build from source, clone the latest version from this repository into your workspace and compile the package using 96 | 97 | cd catkin_workspace/src 98 | git clone https://github.com/nobleo/full_coverage_path_planner.git 99 | cd ../ 100 | catkin_make 101 | 102 | ### Unit Tests 103 | 104 | All tests can be run using: 105 | 106 | catkin build full_coverage_path_planner --catkin-make-args run_tests 107 | 108 | #### test_common 109 | Unit test that checks the basic functions used by the repository 110 | 111 | #### test_spiral_stc 112 | 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. 113 | 114 | #### test_full_coverage_path_planner.test 115 | 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. 116 | 117 | 118 | ## Usage 119 | 120 | Run a full navigation example using: 121 | 122 | roslaunch full_coverage_path_planner test_full_coverage_path_planner.launch 123 | 124 | Give a 2D-goal in rviz to start path planning algorithm 125 | 126 | Depends on: 127 | 128 | [mobile_robot_simulator](https://github.com/mrath/mobile_robot_simulator.git) that integrates /cmd_vel into a base_link TF-frame and an odometry publisher 129 | 130 | [tracking_pid](https://github.com/nobleo/tracking_pid/) Global path tracking controller 131 | 132 | 133 | ## Launch files 134 | 135 | ### test/full_coverage_path_planner/test_full_coverage_path_planner.launch 136 | 137 | Runs the full_coverage_path_planner global planner in combination with tracking PID local planner. 138 | Moreover a coverage progress tracking node is launched to monitor the coverage progress. 139 | Mobile_robot_simulator is used to integrate cmd_vel output into TF and odometry. 140 | 141 | Arguments: 142 | 143 | * **`map`**: path to a global costmap. Default: `$(find full_coverage_path_planner)/maps/basement.yaml)` 144 | * **`target_x_vel`**: target x velocity for use in interpolator. Default: `0.2` 145 | * **`target_yaw_vel`**: target yaw velocity for use in interpolator. Default: `0.2` 146 | * **`robot_radius`**: radius of the robot for use in the global planner. Default: `0.6` 147 | * **`tool_radius`**: radius of the tool for use in the global planner. Default: `0.2` 148 | 149 | 150 | Start planning and tracking by giving a 2D nav goal. 151 | 152 | 153 | ## Nodes 154 | 155 | ### coverage_progress 156 | The CoverageProgressNode keeps track of coverage progress. It does this by periodically looking up the position of the coverage disk in an occupancy grid. Cells within a radius from this position are 'covered' 157 | 158 | #### Subscribed Topics 159 | 160 | * **`/tf`** ([tf2_msgs/TFMessage]) 161 | ros tf dynamic transformations 162 | * **`/tf_static`** ([tf2_msgs/TFMessage]) 163 | ros tf static transformations 164 | #### Published Topics 165 | 166 | * **`/coverage_grid`** ([nav_msgs/OccupancyGrid]) 167 | occupancy grid to visualize coverage progress 168 | * **`/coverage_progress`** ([std_msgs/Float32]) 169 | monitors coverage (from 0 none to 1 full) on the given area 170 | 171 | #### Services 172 | 173 | * **`/coverage_progress/reset`** ([std_srvs/SetBool]) 174 | resets coverage_progress node. For instance when robot position needs to be manually updated 175 | 176 | 177 | #### Parameters 178 | 179 | * **`target_area/x`**: size in x of the target area to monitor 180 | * **`target_area/y`**: size in y of the target area to monitor 181 | * **`coverage_radius`**: radius of the tool to compute coverage progress 182 | 183 | 184 | ## Plugins 185 | ### full_coverage_path_planner/SpiralSTC 186 | 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. 187 | 188 | #### Parameters 189 | 190 | * **`robot_radius`**: robot radius, which is used by the CPP algorithm to check for collisions with static map 191 | * **`tool_radius`**: tool radius, which is used by the CPP algorithm to discretize the space and find a full coverage plan 192 | 193 | 194 | ## References 195 | 196 | [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. 197 | 198 | ## Bugs & Feature Requests 199 | 200 | Please report bugs and request features using the [Issue Tracker](https://github.com/nobleo/full_coverage_path_planner/issues). 201 | 202 | 203 | [ROS]: http://www.ros.org 204 | [rviz]: http://wiki.ros.org/rviz 205 | [MBF]: http://wiki.ros.org/move_base_flex 206 | 207 | ## Acknowledgments 208 | 209 | 213 | 214 | 215 | rosin_logo 217 | 218 | 219 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 220 | More information: rosin-project.eu 221 | 222 | eu_flag 224 | 225 | This project has received funding from the European Union’s Horizon 2020 226 | research and innovation programme under grant agreement no. 732287. 227 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/fcpp.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 554 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | - Class: rviz/Time 23 | Experimental: false 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: "" 27 | Preferences: 28 | PromptSaveOnExit: true 29 | Toolbars: 30 | toolButtonStyle: 2 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Class: rviz/TF 53 | Enabled: true 54 | Frame Timeout: 15 55 | Frames: 56 | All Enabled: true 57 | base_link: 58 | Value: true 59 | coverage_map: 60 | Value: true 61 | map: 62 | Value: true 63 | odom: 64 | Value: true 65 | Marker Scale: 1 66 | Name: TF 67 | Show Arrows: true 68 | Show Axes: true 69 | Show Names: true 70 | Tree: 71 | map: 72 | coverage_map: 73 | {} 74 | odom: 75 | base_link: 76 | {} 77 | Update Interval: 0 78 | Value: true 79 | - Alpha: 0.30000001192092896 80 | Class: rviz/Map 81 | Color Scheme: map 82 | Draw Behind: false 83 | Enabled: true 84 | Name: Map 85 | Topic: /map 86 | Unreliable: false 87 | Use Timestamp: false 88 | Value: true 89 | - Alpha: 1 90 | Buffer Length: 1 91 | Class: rviz/Path 92 | Color: 0; 0; 255 93 | Enabled: true 94 | Head Diameter: 0.05000000074505806 95 | Head Length: 0.019999999552965164 96 | Length: 0.30000001192092896 97 | Line Style: Lines 98 | Line Width: 0.029999999329447746 99 | Name: Path 100 | Offset: 101 | X: 0 102 | Y: 0 103 | Z: 0 104 | Pose Color: 255; 85; 255 105 | Pose Style: Arrows 106 | Radius: 0.029999999329447746 107 | Shaft Diameter: 0.019999999552965164 108 | Shaft Length: 0.05000000074505806 109 | Topic: /move_base/UTurn/plan 110 | Unreliable: false 111 | Value: true 112 | - Angle Tolerance: 0.10000000149011612 113 | Class: rviz/Odometry 114 | Covariance: 115 | Orientation: 116 | Alpha: 0.5 117 | Color: 255; 255; 127 118 | Color Style: Unique 119 | Frame: Local 120 | Offset: 1 121 | Scale: 1 122 | Value: true 123 | Position: 124 | Alpha: 0.30000001192092896 125 | Color: 204; 51; 204 126 | Scale: 1 127 | Value: true 128 | Value: true 129 | Enabled: true 130 | Keep: 10000 131 | Name: Odometry 132 | Position Tolerance: 0.10000000149011612 133 | Shape: 134 | Alpha: 1 135 | Axes Length: 1 136 | Axes Radius: 0.10000000149011612 137 | Color: 255; 25; 0 138 | Head Length: 0.019999999552965164 139 | Head Radius: 0.05000000074505806 140 | Shaft Length: 0.05000000074505806 141 | Shaft Radius: 0.019999999552965164 142 | Value: Arrow 143 | Topic: /odom 144 | Unreliable: false 145 | Value: true 146 | - Class: rviz/Marker 147 | Enabled: true 148 | Marker Topic: /visualization_marker 149 | Name: Tracking_pid local planner 150 | Namespaces: 151 | axle point: true 152 | control point: true 153 | goal point: true 154 | Queue Size: 100 155 | Value: true 156 | - Class: rviz/Marker 157 | Enabled: true 158 | Marker Topic: /move_base_flex/tracking_pid/interpolator 159 | Name: Interpolator (mbf) 160 | Namespaces: 161 | {} 162 | Queue Size: 100 163 | Value: true 164 | - Alpha: 0.30000001192092896 165 | Class: rviz/Map 166 | Color Scheme: map 167 | Draw Behind: false 168 | Enabled: true 169 | Name: Coverage progress map 170 | Topic: /coverage_grid 171 | Unreliable: false 172 | Use Timestamp: false 173 | Value: true 174 | - Class: rviz/Marker 175 | Enabled: true 176 | Marker Topic: /move_base/TrackingPidLocalPlanner/interpolator 177 | Name: Interpolator (mb) 178 | Namespaces: 179 | {} 180 | Queue Size: 100 181 | Value: true 182 | - Alpha: 1 183 | Class: rviz/Polygon 184 | Color: 25; 255; 0 185 | Enabled: true 186 | Name: Footprint 187 | Topic: /move_base_flex/global_costmap/footprint 188 | Unreliable: false 189 | Value: true 190 | Enabled: true 191 | Global Options: 192 | Background Color: 48; 48; 48 193 | Default Light: true 194 | Fixed Frame: map 195 | Frame Rate: 30 196 | Name: root 197 | Tools: 198 | - Class: rviz/Interact 199 | Hide Inactive Objects: true 200 | - Class: rviz/MoveCamera 201 | - Class: rviz/Select 202 | - Class: rviz/FocusCamera 203 | - Class: rviz/Measure 204 | - Class: rviz/SetInitialPose 205 | Theta std deviation: 0.2617993950843811 206 | Topic: /set_pose 207 | X std deviation: 0.5 208 | Y std deviation: 0.5 209 | - Class: rviz/SetGoal 210 | Topic: /move_base_simple/goal 211 | - Class: rviz/PublishPoint 212 | Single click: true 213 | Topic: /clicked_point 214 | Value: true 215 | Views: 216 | Current: 217 | Angle: -3.1400022506713867 218 | Class: rviz/TopDownOrtho 219 | Enable Stereo Rendering: 220 | Stereo Eye Separation: 0.05999999865889549 221 | Stereo Focal Distance: 1 222 | Swap Stereo Eyes: false 223 | Value: false 224 | Invert Z Axis: false 225 | Name: Current View 226 | Near Clip Distance: 0.009999999776482582 227 | Scale: 97.19247436523438 228 | Target Frame: 229 | Value: TopDownOrtho (rviz) 230 | X: -0.14492475986480713 231 | Y: 1.0135213136672974 232 | Saved: ~ 233 | Window Geometry: 234 | Displays: 235 | collapsed: false 236 | Height: 1023 237 | Hide Left Dock: false 238 | Hide Right Dock: true 239 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000361fc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a60000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e9000002b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 240 | Selection: 241 | collapsed: false 242 | Time: 243 | collapsed: true 244 | Tool Properties: 245 | collapsed: true 246 | Views: 247 | collapsed: true 248 | Width: 927 249 | X: 67 250 | Y: 27 251 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/rviz_aco.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 554 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | - Class: rviz/Time 23 | Experimental: false 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: "" 27 | Preferences: 28 | PromptSaveOnExit: true 29 | Toolbars: 30 | toolButtonStyle: 2 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Class: rviz/TF 53 | Enabled: true 54 | Frame Timeout: 15 55 | Frames: 56 | All Enabled: true 57 | base_link: 58 | Value: true 59 | coverage_map: 60 | Value: true 61 | map: 62 | Value: true 63 | odom: 64 | Value: true 65 | Marker Scale: 1 66 | Name: TF 67 | Show Arrows: true 68 | Show Axes: true 69 | Show Names: true 70 | Tree: 71 | map: 72 | coverage_map: 73 | {} 74 | odom: 75 | base_link: 76 | {} 77 | Update Interval: 0 78 | Value: true 79 | - Alpha: 0.30000001192092896 80 | Class: rviz/Map 81 | Color Scheme: map 82 | Draw Behind: false 83 | Enabled: true 84 | Name: Map 85 | Topic: /map 86 | Unreliable: false 87 | Use Timestamp: false 88 | Value: true 89 | - Alpha: 1 90 | Buffer Length: 1 91 | Class: rviz/Path 92 | Color: 0; 0; 255 93 | Enabled: true 94 | Head Diameter: 0.05000000074505806 95 | Head Length: 0.019999999552965164 96 | Length: 0.30000001192092896 97 | Line Style: 98 | Line Width: 0.1 99 | Value: Lines 100 | Shape: 101 | Alpha: 1 102 | Axes Length: 1 103 | Axes Radius: 0.10000000149011612 104 | Color: 255; 25; 0 105 | Head Length: 0.019999999552965164 106 | Head Radius: 0.05000000074505806 107 | Shaft Length: 0.05000000074505806 108 | Shaft Radius: 0.019999999552965164 109 | Value: Arrow 110 | Name: Path 111 | Offset: 112 | X: 0 113 | Y: 0 114 | Z: 0 115 | Pose Color: 255; 85; 255 116 | Pose Style: Arrows 117 | Radius: 0.029999999329447746 118 | Shaft Diameter: 0.019999999552965164 119 | Shaft Length: 0.05000000074505806 120 | Topic: /move_base/AntColonyOptimization/plan 121 | Unreliable: false 122 | Value: true 123 | - Angle Tolerance: 0.10000000149011612 124 | Class: rviz/Odometry 125 | Covariance: 126 | Orientation: 127 | Alpha: 0.5 128 | Color: 255; 255; 127 129 | Color Style: Unique 130 | Frame: Local 131 | Offset: 1 132 | Scale: 1 133 | Value: true 134 | Position: 135 | Alpha: 0.30000001192092896 136 | Color: 204; 51; 204 137 | Scale: 1 138 | Value: true 139 | Value: true 140 | Enabled: true 141 | Keep: 10000 142 | Name: Odometry 143 | Position Tolerance: 0.10000000149011612 144 | Shape: 145 | Alpha: 1 146 | Axes Length: 1 147 | Axes Radius: 0.10000000149011612 148 | Color: 255; 25; 0 149 | Head Length: 0.019999999552965164 150 | Head Radius: 0.05000000074505806 151 | Shaft Length: 0.05000000074505806 152 | Shaft Radius: 0.019999999552965164 153 | Value: Arrow 154 | Topic: /odom 155 | Unreliable: false 156 | Value: true 157 | - Class: rviz/Marker 158 | Enabled: true 159 | Marker Topic: /visualization_marker 160 | Name: Tracking_pid local planner 161 | Namespaces: 162 | axle point: true 163 | control point: true 164 | goal point: true 165 | Queue Size: 100 166 | Value: true 167 | - Class: rviz/Marker 168 | Enabled: true 169 | Marker Topic: /move_base_flex/tracking_pid/interpolator 170 | Name: Interpolator (mbf) 171 | Namespaces: 172 | {} 173 | Queue Size: 100 174 | Value: true 175 | - Alpha: 0.30000001192092896 176 | Class: rviz/Map 177 | Color Scheme: map 178 | Draw Behind: false 179 | Enabled: true 180 | Name: Coverage progress map 181 | Topic: /coverage_grid 182 | Unreliable: false 183 | Use Timestamp: false 184 | Value: true 185 | - Class: rviz/Marker 186 | Enabled: true 187 | Marker Topic: /move_base/TrackingPidLocalPlanner/interpolator 188 | Name: Interpolator (mb) 189 | Namespaces: 190 | {} 191 | Queue Size: 100 192 | Value: true 193 | - Alpha: 1 194 | Class: rviz/Polygon 195 | Color: 25; 255; 0 196 | Enabled: true 197 | Name: Footprint 198 | Topic: /move_base_flex/global_costmap/footprint 199 | Unreliable: false 200 | Value: true 201 | Enabled: true 202 | Global Options: 203 | Background Color: 48; 48; 48 204 | Default Light: true 205 | Fixed Frame: map 206 | Frame Rate: 30 207 | Name: root 208 | Tools: 209 | - Class: rviz/Interact 210 | Hide Inactive Objects: true 211 | - Class: rviz/MoveCamera 212 | - Class: rviz/Select 213 | - Class: rviz/FocusCamera 214 | - Class: rviz/Measure 215 | - Class: rviz/SetInitialPose 216 | Theta std deviation: 0.2617993950843811 217 | Topic: /set_pose 218 | X std deviation: 0.5 219 | Y std deviation: 0.5 220 | - Class: rviz/SetGoal 221 | Topic: /move_base_simple/goal 222 | - Class: rviz/PublishPoint 223 | Single click: true 224 | Topic: /clicked_point 225 | Value: true 226 | Views: 227 | Current: 228 | Angle: -3.1400022506713867 229 | Class: rviz/TopDownOrtho 230 | Enable Stereo Rendering: 231 | Stereo Eye Separation: 0.05999999865889549 232 | Stereo Focal Distance: 1 233 | Swap Stereo Eyes: false 234 | Value: false 235 | Invert Z Axis: false 236 | Name: Current View 237 | Near Clip Distance: 0.009999999776482582 238 | Scale: 97.19247436523438 239 | Target Frame: 240 | Value: TopDownOrtho (rviz) 241 | X: -0.14492475986480713 242 | Y: 1.0135213136672974 243 | Saved: ~ 244 | Window Geometry: 245 | Displays: 246 | collapsed: false 247 | Height: 1023 248 | Hide Left Dock: false 249 | Hide Right Dock: true 250 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000361fc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a60000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e9000002b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 251 | Selection: 252 | collapsed: false 253 | Time: 254 | collapsed: true 255 | Tool Properties: 256 | collapsed: true 257 | Views: 258 | collapsed: true 259 | Width: 927 260 | X: 67 261 | Y: 27 262 | -------------------------------------------------------------------------------- /doc/robot_plus_tool.ipe: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 0 0 m 9 | -1 0.333 l 10 | -1 -0.333 l 11 | h 12 | 13 | 14 | 15 | 16 | 0 0 m 17 | -1 0.333 l 18 | -1 -0.333 l 19 | h 20 | 21 | 22 | 23 | 24 | 0 0 m 25 | -1 0.333 l 26 | -0.8 0 l 27 | -1 -0.333 l 28 | h 29 | 30 | 31 | 32 | 33 | 0 0 m 34 | -1 0.333 l 35 | -0.8 0 l 36 | -1 -0.333 l 37 | h 38 | 39 | 40 | 41 | 42 | 0.6 0 0 0.6 0 0 e 43 | 0.4 0 0 0.4 0 0 e 44 | 45 | 46 | 47 | 48 | 0.6 0 0 0.6 0 0 e 49 | 50 | 51 | 52 | 53 | 54 | 0.5 0 0 0.5 0 0 e 55 | 56 | 57 | 0.6 0 0 0.6 0 0 e 58 | 0.4 0 0 0.4 0 0 e 59 | 60 | 61 | 62 | 63 | 64 | -0.6 -0.6 m 65 | 0.6 -0.6 l 66 | 0.6 0.6 l 67 | -0.6 0.6 l 68 | h 69 | -0.4 -0.4 m 70 | 0.4 -0.4 l 71 | 0.4 0.4 l 72 | -0.4 0.4 l 73 | h 74 | 75 | 76 | 77 | 78 | -0.6 -0.6 m 79 | 0.6 -0.6 l 80 | 0.6 0.6 l 81 | -0.6 0.6 l 82 | h 83 | 84 | 85 | 86 | 87 | 88 | -0.5 -0.5 m 89 | 0.5 -0.5 l 90 | 0.5 0.5 l 91 | -0.5 0.5 l 92 | h 93 | 94 | 95 | -0.6 -0.6 m 96 | 0.6 -0.6 l 97 | 0.6 0.6 l 98 | -0.6 0.6 l 99 | h 100 | -0.4 -0.4 m 101 | 0.4 -0.4 l 102 | 0.4 0.4 l 103 | -0.4 0.4 l 104 | h 105 | 106 | 107 | 108 | 109 | 110 | 111 | -0.43 -0.57 m 112 | 0.57 0.43 l 113 | 0.43 0.57 l 114 | -0.57 -0.43 l 115 | h 116 | 117 | 118 | -0.43 0.57 m 119 | 0.57 -0.43 l 120 | 0.43 -0.57 l 121 | -0.57 0.43 l 122 | h 123 | 124 | 125 | 126 | 127 | 128 | 0 0 m 129 | -1 0.333 l 130 | -1 -0.333 l 131 | h 132 | 133 | 134 | 135 | 136 | 0 0 m 137 | -1 0.333 l 138 | -0.8 0 l 139 | -1 -0.333 l 140 | h 141 | 142 | 143 | 144 | 145 | 0 0 m 146 | -1 0.333 l 147 | -0.8 0 l 148 | -1 -0.333 l 149 | h 150 | 151 | 152 | 153 | 154 | -1 0.333 m 155 | 0 0 l 156 | -1 -0.333 l 157 | 158 | 159 | 160 | 161 | 0 0 m 162 | -1 0.333 l 163 | -1 -0.333 l 164 | h 165 | -1 0 m 166 | -2 0.333 l 167 | -2 -0.333 l 168 | h 169 | 170 | 171 | 172 | 173 | 0 0 m 174 | -1 0.333 l 175 | -1 -0.333 l 176 | h 177 | -1 0 m 178 | -2 0.333 l 179 | -2 -0.333 l 180 | h 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 176 752 m 259 | 176 688 l 260 | 304 688 l 261 | 304 752 l 262 | h 263 | 264 | 265 | 192 768 m 266 | 192 752 l 267 | 224 752 l 268 | 224 768 l 269 | h 270 | 271 | 272 | 192 688 m 273 | 192 672 l 274 | 224 672 l 275 | 224 688 l 276 | h 277 | 278 | 279 | 31.241 0 0 31.241 240 720 e 280 | 281 | 282 | 71.5542 0 0 71.5542 240 720 e 283 | 284 | 285 | 276 724 m 286 | 276 716 l 287 | 292 716 l 288 | 292 724 l 289 | h 290 | 291 | 292 | 293 | 284 720 m 294 | 296 720 l 295 | 296 | 297 | 240 724 m 298 | 240 716 l 299 | 300 | 301 | 236 720 m 302 | 244 720 l 303 | 304 | robot radius 305 | tool radius 306 | 307 | 263.673 739.795 m 308 | 294.193 766.331 l 309 | 322.062 766.258 l 310 | 311 | 312 | 270.306 784.565 m 313 | 270.476 784.41 l 314 | 321.523 784.366 l 315 | 316 | 317 | 239.996 720.147 m 318 | 270.197 784.118 l 319 | 320 | 321 | 240.045 719.919 m 322 | 263.539 740.101 l 323 | 324 | 325 | 326 | -------------------------------------------------------------------------------- /src/full_coverage_path_planner.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | 7 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 8 | 9 | /* *** Note the coordinate system *** 10 | * grid[][] is a 2D-vector: 11 | * where ix is column-index and x-coordinate in map, 12 | * iy is row-index and y-coordinate in map. 13 | * 14 | * Cols [ix] 15 | * _______________________ 16 | * |__|__|__|__|__|__|__|__| 17 | * |__|__|__|__|__|__|__|__| 18 | * Rows |__|__|__|__|__|__|__|__| 19 | * [iy] |__|__|__|__|__|__|__|__| 20 | * |__|__|__|__|__|__|__|__| 21 | *y-axis |__|__|__|__|__|__|__|__| 22 | * ^ |__|__|__|__|__|__|__|__| 23 | * ^ |__|__|__|__|__|__|__|__| 24 | * | |__|__|__|__|__|__|__|__| 25 | * | |__|__|__|__|__|__|__|__| 26 | * 27 | * O --->> x-axis 28 | */ 29 | 30 | // #define DEBUG_PLOT 31 | 32 | // Default Constructor 33 | namespace full_coverage_path_planner 34 | { 35 | FullCoveragePathPlanner::FullCoveragePathPlanner() : initialized_(false) 36 | { 37 | } 38 | 39 | void FullCoveragePathPlanner::publishPlan(const std::vector& path) 40 | { 41 | if (!initialized_) 42 | { 43 | ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 44 | return; 45 | } 46 | 47 | // create a message for the plan 48 | nav_msgs::Path gui_path; 49 | gui_path.poses.resize(path.size()); 50 | 51 | if (!path.empty()) 52 | { 53 | gui_path.header.frame_id = path[0].header.frame_id; 54 | gui_path.header.stamp = path[0].header.stamp; 55 | } 56 | 57 | // Extract the plan in world co-ordinates, we assume the path is all in the same frame 58 | for (unsigned int i = 0; i < path.size(); i++) 59 | { 60 | gui_path.poses[i] = path[i]; 61 | } 62 | 63 | plan_pub_.publish(gui_path); 64 | } 65 | 66 | void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamped& start, 67 | std::list const& goalpoints, 68 | std::vector& plan) 69 | { 70 | geometry_msgs::PoseStamped new_goal; 71 | std::list::const_iterator it, it_next, it_prev; 72 | int dx_now, dy_now, dx_next, dy_next, move_dir_now = 0, move_dir_prev = 0, move_dir_next = 0; 73 | bool do_publish = false; 74 | float orientation = eDirNone; 75 | ROS_INFO("Received goalpoints with length: %lu", goalpoints.size()); 76 | if (goalpoints.size() > 1) 77 | { 78 | for (it = goalpoints.begin(); it != goalpoints.end(); ++it) 79 | { 80 | it_next = it; 81 | it_next++; 82 | it_prev = it; 83 | it_prev--; 84 | 85 | // Check for the direction of movement 86 | if (it == goalpoints.begin()) 87 | { 88 | dx_now = it_next->x - it->x; 89 | dy_now = it_next->y - it->y; 90 | } 91 | else 92 | { 93 | dx_now = it->x - it_prev->x; 94 | dy_now = it->y - it_prev->y; 95 | dx_next = it_next->x - it->x; 96 | dy_next = it_next->y - it->y; 97 | } 98 | 99 | // Calculate direction enum: dx + dy*2 will give a unique number for each of the four possible directions because 100 | // of their signs: 101 | // 1 + 0*2 = 1 102 | // 0 + 1*2 = 2 103 | // -1 + 0*2 = -1 104 | // 0 + -1*2 = -2 105 | move_dir_now = dx_now + dy_now * 2; 106 | move_dir_next = dx_next + dy_next * 2; 107 | 108 | // Check if this points needs to be published (i.e. a change of direction or first or last point in list) 109 | do_publish = move_dir_next != move_dir_now || it == goalpoints.begin() || 110 | (it != goalpoints.end() && it == --goalpoints.end()); 111 | move_dir_prev = move_dir_now; 112 | 113 | // Add to vector if required 114 | if (do_publish) 115 | { 116 | new_goal.header.frame_id = "map"; 117 | new_goal.pose.position.x = (it->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; 118 | new_goal.pose.position.y = (it->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; 119 | // Calculate desired orientation to be in line with movement direction 120 | switch (move_dir_now) 121 | { 122 | case eDirNone: 123 | // Keep orientation 124 | break; 125 | case eDirRight: 126 | orientation = 0; 127 | break; 128 | case eDirUp: 129 | orientation = M_PI / 2; 130 | break; 131 | case eDirLeft: 132 | orientation = M_PI; 133 | break; 134 | case eDirDown: 135 | orientation = M_PI * 1.5; 136 | break; 137 | } 138 | new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(orientation); 139 | if (it != goalpoints.begin()) 140 | { 141 | previous_goal_.pose.orientation = new_goal.pose.orientation; 142 | // republish previous goal but with new orientation to indicate change of direction 143 | // useful when the plan is strictly followed with base_link 144 | plan.push_back(previous_goal_); 145 | } 146 | ROS_DEBUG("Voila new point: x=%f, y=%f, o=%f,%f,%f,%f", new_goal.pose.position.x, new_goal.pose.position.y, 147 | new_goal.pose.orientation.x, new_goal.pose.orientation.y, new_goal.pose.orientation.z, 148 | new_goal.pose.orientation.w); 149 | plan.push_back(new_goal); 150 | previous_goal_ = new_goal; 151 | } 152 | } 153 | } 154 | else 155 | { 156 | new_goal.header.frame_id = "map"; 157 | new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; 158 | new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; 159 | new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(0); 160 | plan.push_back(new_goal); 161 | } 162 | /* Add poses from current position to start of plan */ 163 | 164 | // Compute angle between current pose and first plan point 165 | double dy = plan.begin()->pose.position.y - start.pose.position.y; 166 | double dx = plan.begin()->pose.position.x - start.pose.position.x; 167 | // Arbitrary choice of 100.0*FLT_EPSILON to determine minimum angle precision of 1% 168 | if (!(fabs(dy) < 100.0 * FLT_EPSILON && fabs(dx) < 100.0 * FLT_EPSILON)) 169 | { 170 | // Add extra translation waypoint 171 | double yaw = std::atan2(dy, dx); 172 | geometry_msgs::Quaternion quat_temp = tf::createQuaternionMsgFromYaw(yaw); 173 | geometry_msgs::PoseStamped extra_pose; 174 | extra_pose = *plan.begin(); 175 | extra_pose.pose.orientation = quat_temp; 176 | plan.insert(plan.begin(), extra_pose); 177 | extra_pose = start; 178 | extra_pose.pose.orientation = quat_temp; 179 | plan.insert(plan.begin(), extra_pose); 180 | } 181 | 182 | // Insert current pose 183 | plan.insert(plan.begin(), start); 184 | 185 | ROS_INFO("Plan ready containing %lu goals!", plan.size()); 186 | } 187 | 188 | bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_, 189 | std::vector >& grid, 190 | float robotRadius, 191 | float toolRadius, 192 | geometry_msgs::PoseStamped const& realStart, 193 | Point_t& scaledStart) 194 | { 195 | int ix, iy, nodeRow, nodeColl; 196 | uint32_t nodeSize = dmax(floor(toolRadius / cpp_grid_.info.resolution), 1); // Size of node in pixels/units 197 | uint32_t robotNodeSize = dmax(floor(robotRadius / cpp_grid_.info.resolution), 1); // RobotRadius in pixels/units 198 | uint32_t nRows = cpp_grid_.info.height, nCols = cpp_grid_.info.width; 199 | ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize); 200 | 201 | if (nRows == 0 || nCols == 0) 202 | { 203 | return false; 204 | } 205 | 206 | // Save map origin and scaling 207 | tile_size_ = nodeSize * cpp_grid_.info.resolution; // Size of a tile in meters 208 | grid_origin_.x = cpp_grid_.info.origin.position.x; // x-origin in meters 209 | grid_origin_.y = cpp_grid_.info.origin.position.y; // y-origin in meters 210 | 211 | // Scale starting point 212 | scaledStart.x = static_cast(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0, 213 | floor(cpp_grid_.info.width / tile_size_))); 214 | scaledStart.y = static_cast(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0, 215 | floor(cpp_grid_.info.height / tile_size_))); 216 | 217 | // Scale grid 218 | for (iy = 0; iy < nRows; iy = iy + nodeSize) 219 | { 220 | std::vector gridRow; 221 | for (ix = 0; ix < nCols; ix = ix + nodeSize) 222 | { 223 | bool nodeOccupied = false; 224 | for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) 225 | { 226 | for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl) 227 | { 228 | int index_grid = dmax((iy + nodeRow - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)) 229 | * nCols + (ix + nodeColl - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)), 0); 230 | if (cpp_grid_.data[index_grid] > 65) 231 | { 232 | nodeOccupied = true; 233 | break; 234 | } 235 | } 236 | } 237 | gridRow.push_back(nodeOccupied); 238 | } 239 | grid.push_back(gridRow); 240 | } 241 | return true; 242 | } 243 | } // namespace full_coverage_path_planner 244 | -------------------------------------------------------------------------------- /src/uturn.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2022] Manchester Metropolitian University" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "full_coverage_path_planner/uturn.h" 11 | #include 12 | 13 | // register this planner as a BaseGlobalPlanner plugin 14 | PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::UTurn, nav_core::BaseGlobalPlanner) 15 | 16 | namespace full_coverage_path_planner 17 | { 18 | void UTurn::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 19 | { 20 | if (!initialized_) 21 | { 22 | // Create a publisher to visualize the plan 23 | ros::NodeHandle private_nh("~/"); 24 | ros::NodeHandle nh, private_named_nh("~/" + name); 25 | 26 | plan_pub_ = private_named_nh.advertise("plan", 1); 27 | // Try to request the cpp-grid from the cpp_grid map_server 28 | cpp_grid_client_ = nh.serviceClient("static_map"); 29 | 30 | // Define robot radius (radius) parameter 31 | float robot_radius_default = 0.5f; 32 | private_named_nh.param("robot_radius", robot_radius_, robot_radius_default); 33 | // Define tool radius (radius) parameter 34 | float tool_radius_default = 0.5f; 35 | private_named_nh.param("tool_radius", tool_radius_, tool_radius_default); 36 | initialized_ = true; 37 | } 38 | } 39 | 40 | std::list UTurn::move(std::vector > const& grid, std::list& init, 41 | std::vector >& visited) 42 | { 43 | int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size(); 44 | // Spiral filling of the open space 45 | // Copy incoming list to 'end' 46 | std::list pathNodes(init); 47 | // Create iterator for gridNode_t list and let it point to the last element of end 48 | std::list::iterator it = --(pathNodes.end()); 49 | if (pathNodes.size() > 1) // if list is length 1, keep iterator at end 50 | it--; // Let iterator point to second to last element 51 | 52 | gridNode_t prev = *(it); 53 | bool done = false; 54 | while (!done) 55 | { 56 | dx = 0; 57 | dy = 1; 58 | done = true; 59 | 60 | for (int i = 0; i < 4; ++i) 61 | { 62 | x2 = pathNodes.back().pos.x + dx; 63 | y2 = pathNodes.back().pos.y + dy; 64 | if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) 65 | { 66 | if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen) 67 | { 68 | Point_t new_point = { x2, y2 }; 69 | gridNode_t new_node = 70 | { 71 | new_point, // Point: x,y 72 | 0, // Cost 73 | 0, // Heuristic 74 | }; 75 | prev = pathNodes.back(); 76 | pathNodes.push_back(new_node); 77 | it = --(pathNodes.end()); 78 | visited[y2][x2] = eNodeVisited; // Close node 79 | done = false; 80 | break; 81 | } 82 | } 83 | // try next direction cw 84 | dx_prev = dx; 85 | dx = dy; 86 | dy = -dx_prev; 87 | } 88 | } 89 | return pathNodes; 90 | } 91 | 92 | std::list UTurn::moveFull(std::vector > const& grid, 93 | Point_t& init, 94 | int &multiple_pass_counter, 95 | int &visited_counter) 96 | { 97 | int x, y, nRows = grid.size(), nCols = grid[0].size(); 98 | // Initial node is initially set as visited so it does not count 99 | multiple_pass_counter = 0; 100 | visited_counter = 0; 101 | 102 | std::vector > visited; 103 | visited = grid; // Copy grid matrix 104 | x = init.x; 105 | y = init.y; 106 | 107 | Point_t new_point = { x, y }; 108 | gridNode_t new_node = 109 | { 110 | new_point, // Point: x,y 111 | 0, // Cost 112 | 0, // Heuristic 113 | }; 114 | std::list pathNodes; 115 | std::list fullPath; 116 | pathNodes.push_back(new_node); 117 | visited[y][x] = eNodeVisited; 118 | 119 | #ifdef DEBUG_PLOT 120 | ROS_INFO("Grid before walking is: "); 121 | printGrid(grid, visited, fullPath); 122 | #endif 123 | 124 | std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints 125 | // Add points to full path 126 | std::list::iterator it; 127 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 128 | { 129 | Point_t newPoint = { it->pos.x, it->pos.y }; 130 | visited_counter++; 131 | fullPath.push_back(newPoint); 132 | } 133 | 134 | #ifdef DEBUG_PLOT 135 | ROS_INFO("Current grid after first spiral is"); 136 | printGrid(grid, visited, fullPath); 137 | ROS_INFO("There are %d goals remaining", goals.size()); 138 | #endif 139 | while (goals.size() != 0) 140 | { 141 | // Remove all elements from pathNodes list except last element. 142 | // The last point is the starting point for a new search and A* extends the path from there on 143 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 144 | visited_counter--; // First point is already counted as visited 145 | // Plan to closest open Node using A* 146 | // `goals` is essentially the map, so we use `goals` to determine the distance from the end of a potential path 147 | // to the nearest free space 148 | bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes); 149 | if (resign) 150 | { 151 | #ifdef DEBUG_PLOT 152 | ROS_INFO("A_star_to_open_space is resigning", goals.size()); 153 | #endif 154 | break; 155 | } 156 | 157 | // Update visited grid 158 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 159 | { 160 | if (visited[it->pos.y][it->pos.x]) 161 | { 162 | multiple_pass_counter++; 163 | } 164 | visited[it->pos.y][it->pos.x] = eNodeVisited; 165 | } 166 | if (pathNodes.size() > 0) 167 | { 168 | multiple_pass_counter--; // First point is already counted as visited 169 | } 170 | 171 | #ifdef DEBUG_PLOT 172 | ROS_INFO("Grid with path marked as visited is:"); 173 | gridNode_t SpiralStart = pathNodes.back(); 174 | printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back()); 175 | #endif 176 | 177 | // Spiral fill from current position 178 | pathNodes = move(grid, pathNodes, visited); 179 | 180 | #ifdef DEBUG_PLOT 181 | ROS_INFO("Visited grid updated after spiral:"); 182 | printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back()); 183 | #endif 184 | 185 | goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints 186 | 187 | //move ant to it's initial location 188 | visited[init.y][init.x] = eNodeOpen; 189 | goals.push_back({init.x, init.y}); 190 | a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes); 191 | visited[init.y][init.x] = eNodeVisited; 192 | 193 | 194 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 195 | { 196 | Point_t newPoint = { it->pos.x, it->pos.y }; 197 | visited_counter++; 198 | fullPath.push_back(newPoint); 199 | } 200 | } 201 | 202 | return fullPath; 203 | } 204 | 205 | bool UTurn::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, 206 | std::vector& plan) 207 | { 208 | if (!initialized_) 209 | { 210 | ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 211 | return false; 212 | } 213 | else 214 | { 215 | ROS_INFO("Initialized!"); 216 | } 217 | 218 | clock_t begin = clock(); 219 | Point_t startPoint; 220 | 221 | /********************** Get grid from server **********************/ 222 | std::vector > grid; 223 | nav_msgs::GetMap grid_req_srv; 224 | ROS_INFO("Requesting grid!!"); 225 | if (!cpp_grid_client_.call(grid_req_srv)) 226 | { 227 | ROS_ERROR("Could not retrieve grid from map_server"); 228 | return false; 229 | } 230 | 231 | if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) 232 | { 233 | ROS_ERROR("Could not parse retrieved grid"); 234 | return false; 235 | } 236 | 237 | #ifdef DEBUG_PLOT 238 | ROS_INFO("Start grid is:"); 239 | std::list printPath; 240 | printPath.push_back(startPoint); 241 | printGrid(grid, grid, printPath); 242 | #endif 243 | 244 | std::list goalPoints = moveFull(grid, 245 | startPoint, 246 | spiral_cpp_metrics_.multiple_pass_counter, 247 | spiral_cpp_metrics_.visited_counter); 248 | ROS_INFO("naive cpp completed!"); 249 | ROS_INFO("Converting path to plan"); 250 | 251 | 252 | clock_t end = clock(); 253 | float elapsed_secs = static_cast((end - begin) / CLOCKS_PER_SEC)*1000; 254 | 255 | parsePointlist2Plan(start, goalPoints, plan); 256 | // Print some metrics: 257 | spiral_cpp_metrics_.multiple_pass_counter = 0; 258 | spiral_cpp_metrics_.visited_counter = 0; 259 | auto test_grid(grid); 260 | for (Point_t nodeOnPath : goalPoints) 261 | { 262 | spiral_cpp_metrics_.visited_counter++; 263 | if(test_grid[nodeOnPath.y][nodeOnPath.x] == eNodeVisited) 264 | { 265 | spiral_cpp_metrics_.multiple_pass_counter++; 266 | } 267 | else 268 | { 269 | test_grid[nodeOnPath.y][nodeOnPath.x] = eNodeVisited; 270 | 271 | } 272 | } 273 | ROS_INFO("Total visited: %d", spiral_cpp_metrics_.visited_counter); 274 | ROS_INFO("Total re-visited: %d", spiral_cpp_metrics_.multiple_pass_counter); 275 | 276 | // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept 277 | // (also controlled by planner_frequency parameter in move_base namespace) 278 | 279 | ROS_INFO("Publishing plan!"); 280 | publishPlan(plan); 281 | ROS_INFO("Plan published!"); 282 | ROS_DEBUG("Plan published"); 283 | 284 | std::cout << "elapsed time: " << elapsed_secs << "\n"; 285 | 286 | return true; 287 | } 288 | } // namespace full_coverage_path_planner 289 | -------------------------------------------------------------------------------- /src/spiral_stc.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "full_coverage_path_planner/spiral_stc.h" 11 | #include 12 | 13 | // register this planner as a BaseGlobalPlanner plugin 14 | PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::SpiralSTC, nav_core::BaseGlobalPlanner) 15 | 16 | namespace full_coverage_path_planner 17 | { 18 | void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 19 | { 20 | if (!initialized_) 21 | { 22 | // Create a publisher to visualize the plan 23 | ros::NodeHandle private_nh("~/"); 24 | ros::NodeHandle nh, private_named_nh("~/" + name); 25 | 26 | plan_pub_ = private_named_nh.advertise("plan", 1); 27 | // Try to request the cpp-grid from the cpp_grid map_server 28 | cpp_grid_client_ = nh.serviceClient("static_map"); 29 | 30 | // Define robot radius (radius) parameter 31 | float robot_radius_default = 0.5f; 32 | private_named_nh.param("robot_radius", robot_radius_, robot_radius_default); 33 | // Define tool radius (radius) parameter 34 | float tool_radius_default = 0.5f; 35 | private_named_nh.param("tool_radius", tool_radius_, tool_radius_default); 36 | initialized_ = true; 37 | } 38 | } 39 | 40 | std::list SpiralSTC::spiral(std::vector > const& grid, std::list& init, 41 | std::vector >& visited) 42 | { 43 | int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size(); 44 | // Spiral filling of the open space 45 | // Copy incoming list to 'end' 46 | std::list pathNodes(init); 47 | // Create iterator for gridNode_t list and let it point to the last element of end 48 | std::list::iterator it = --(pathNodes.end()); 49 | if (pathNodes.size() > 1) // if list is length 1, keep iterator at end 50 | it--; // Let iterator point to second to last element 51 | 52 | gridNode_t prev = *(it); 53 | bool done = false; 54 | while (!done) 55 | { 56 | if (it != pathNodes.begin()) 57 | { 58 | // turn ccw 59 | dx = pathNodes.back().pos.x - prev.pos.x; 60 | dy = pathNodes.back().pos.y - prev.pos.y; 61 | dx_prev = dx; 62 | dx = -dy; 63 | dy = dx_prev; 64 | } 65 | else 66 | { 67 | // Initialize spiral direction towards y-axis 68 | dx = 0; 69 | dy = 1; 70 | } 71 | done = true; 72 | 73 | for (int i = 0; i < 4; ++i) 74 | { 75 | x2 = pathNodes.back().pos.x + dx; 76 | y2 = pathNodes.back().pos.y + dy; 77 | if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) 78 | { 79 | if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen) 80 | { 81 | Point_t new_point = { x2, y2 }; 82 | gridNode_t new_node = 83 | { 84 | new_point, // Point: x,y 85 | 0, // Cost 86 | 0, // Heuristic 87 | }; 88 | prev = pathNodes.back(); 89 | pathNodes.push_back(new_node); 90 | it = --(pathNodes.end()); 91 | visited[y2][x2] = eNodeVisited; // Close node 92 | done = false; 93 | break; 94 | } 95 | } 96 | // try next direction cw 97 | dx_prev = dx; 98 | dx = dy; 99 | dy = -dx_prev; 100 | } 101 | } 102 | return pathNodes; 103 | } 104 | 105 | std::list SpiralSTC::spiral_stc(std::vector > const& grid, 106 | Point_t& init, 107 | int &multiple_pass_counter, 108 | int &visited_counter) 109 | { 110 | int x, y, nRows = grid.size(), nCols = grid[0].size(); 111 | // Initial node is initially set as visited so it does not count 112 | multiple_pass_counter = 0; 113 | visited_counter = 0; 114 | 115 | std::vector > visited; 116 | visited = grid; // Copy grid matrix 117 | x = init.x; 118 | y = init.y; 119 | 120 | Point_t new_point = { x, y }; 121 | gridNode_t new_node = 122 | { 123 | new_point, // Point: x,y 124 | 0, // Cost 125 | 0, // Heuristic 126 | }; 127 | std::list pathNodes; 128 | std::list fullPath; 129 | pathNodes.push_back(new_node); 130 | visited[y][x] = eNodeVisited; 131 | 132 | #ifdef DEBUG_PLOT 133 | ROS_INFO("Grid before walking is: "); 134 | printGrid(grid, visited, fullPath); 135 | #endif 136 | 137 | pathNodes = SpiralSTC::spiral(grid, pathNodes, visited); // First spiral fill 138 | std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints 139 | // Add points to full path 140 | std::list::iterator it; 141 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 142 | { 143 | Point_t newPoint = { it->pos.x, it->pos.y }; 144 | visited_counter++; 145 | fullPath.push_back(newPoint); 146 | } 147 | // Remove all elements from pathNodes list except last element 148 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 149 | 150 | #ifdef DEBUG_PLOT 151 | ROS_INFO("Current grid after first spiral is"); 152 | printGrid(grid, visited, fullPath); 153 | ROS_INFO("There are %d goals remaining", goals.size()); 154 | #endif 155 | while (goals.size() != 0) 156 | { 157 | // Remove all elements from pathNodes list except last element. 158 | // The last point is the starting point for a new search and A* extends the path from there on 159 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 160 | visited_counter--; // First point is already counted as visited 161 | // Plan to closest open Node using A* 162 | // `goals` is essentially the map, so we use `goals` to determine the distance from the end of a potential path 163 | // to the nearest free space 164 | bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes); 165 | if (resign) 166 | { 167 | #ifdef DEBUG_PLOT 168 | ROS_INFO("A_star_to_open_space is resigning", goals.size()); 169 | #endif 170 | break; 171 | } 172 | 173 | // Update visited grid 174 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 175 | { 176 | if (visited[it->pos.y][it->pos.x]) 177 | { 178 | multiple_pass_counter++; 179 | } 180 | visited[it->pos.y][it->pos.x] = eNodeVisited; 181 | } 182 | if (pathNodes.size() > 0) 183 | { 184 | multiple_pass_counter--; // First point is already counted as visited 185 | } 186 | 187 | #ifdef DEBUG_PLOT 188 | ROS_INFO("Grid with path marked as visited is:"); 189 | gridNode_t SpiralStart = pathNodes.back(); 190 | printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back()); 191 | #endif 192 | 193 | // Spiral fill from current position 194 | pathNodes = spiral(grid, pathNodes, visited); 195 | 196 | #ifdef DEBUG_PLOT 197 | ROS_INFO("Visited grid updated after spiral:"); 198 | printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back()); 199 | #endif 200 | 201 | goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints 202 | 203 | //move ant to it's initial location 204 | visited[init.y][init.x] = eNodeOpen; 205 | goals.push_back({init.x, init.y}); 206 | a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes); 207 | visited[init.y][init.x] = eNodeVisited; 208 | 209 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 210 | { 211 | Point_t newPoint = { it->pos.x, it->pos.y }; 212 | visited_counter++; 213 | fullPath.push_back(newPoint); 214 | } 215 | } 216 | 217 | return fullPath; 218 | } 219 | 220 | bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, 221 | std::vector& plan) 222 | { 223 | if (!initialized_) 224 | { 225 | ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 226 | return false; 227 | } 228 | else 229 | { 230 | ROS_INFO("Initialized!"); 231 | } 232 | 233 | clock_t begin = clock(); 234 | Point_t startPoint; 235 | 236 | /********************** Get grid from server **********************/ 237 | std::vector > grid; 238 | nav_msgs::GetMap grid_req_srv; 239 | ROS_INFO("Requesting grid!!"); 240 | if (!cpp_grid_client_.call(grid_req_srv)) 241 | { 242 | ROS_ERROR("Could not retrieve grid from map_server"); 243 | return false; 244 | } 245 | 246 | if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) 247 | { 248 | ROS_ERROR("Could not parse retrieved grid"); 249 | return false; 250 | } 251 | 252 | #ifdef DEBUG_PLOT 253 | ROS_INFO("Start grid is:"); 254 | std::list printPath; 255 | printPath.push_back(startPoint); 256 | printGrid(grid, grid, printPath); 257 | #endif 258 | 259 | std::list goalPoints = spiral_stc(grid, 260 | startPoint, 261 | spiral_cpp_metrics_.multiple_pass_counter, 262 | spiral_cpp_metrics_.visited_counter); 263 | ROS_INFO("naive cpp completed!"); 264 | ROS_INFO("Converting path to plan"); 265 | 266 | parsePointlist2Plan(start, goalPoints, plan); 267 | // Print some metrics: 268 | spiral_cpp_metrics_.multiple_pass_counter = 0; 269 | spiral_cpp_metrics_.visited_counter = 0; 270 | auto test_grid(grid); 271 | for (Point_t nodeOnPath : goalPoints) 272 | { 273 | spiral_cpp_metrics_.visited_counter++; 274 | if(test_grid[nodeOnPath.y][nodeOnPath.x] == eNodeVisited) 275 | { 276 | spiral_cpp_metrics_.multiple_pass_counter++; 277 | } 278 | else 279 | { 280 | test_grid[nodeOnPath.y][nodeOnPath.x] = eNodeVisited; 281 | 282 | } 283 | } 284 | ROS_INFO("Total visited: %d", spiral_cpp_metrics_.visited_counter); 285 | ROS_INFO("Total re-visited: %d", spiral_cpp_metrics_.multiple_pass_counter); 286 | 287 | // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept 288 | // (also controlled by planner_frequency parameter in move_base namespace) 289 | 290 | ROS_INFO("Publishing plan!"); 291 | publishPlan(plan); 292 | ROS_INFO("Plan published!"); 293 | ROS_DEBUG("Plan published"); 294 | 295 | clock_t end = clock(); 296 | double elapsed_secs = static_cast(end - begin) / CLOCKS_PER_SEC; 297 | std::cout << "elapsed time: " << elapsed_secs << "\n"; 298 | 299 | return true; 300 | } 301 | } // namespace full_coverage_path_planner 302 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /src/common.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2022] Manchester Metropolitian University" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | _Float64 score(_Float64 coverage, int accessable_tiles_count) 15 | { 16 | return abs(1/accessable_tiles_count - 1/(coverage*100)); 17 | } 18 | 19 | int distanceToClosestPoint(Point_t poi, std::list const& goals) 20 | { 21 | // Return minimum distance from goals-list 22 | int min_dist = INT_MAX; 23 | std::list::const_iterator it; 24 | for (it = goals.begin(); it != goals.end(); ++it) 25 | { 26 | int cur_dist = distanceSquared((*it), poi); 27 | if (cur_dist < min_dist) 28 | { 29 | min_dist = cur_dist; 30 | } 31 | } 32 | return min_dist; 33 | } 34 | 35 | 36 | static double calculateH(int x, int y, Point_t dest) { 37 | return sqrt( 38 | (x-dest.x)*(x-dest.x) 39 | + 40 | (y-dest.y) * (y-dest.y) 41 | ); 42 | } 43 | 44 | int manhattenDistance(const Point_t& p1, const Point_t& p2) 45 | { 46 | double distance; 47 | int x_dif, y_dif; 48 | x_dif = p2.x - p1.x; 49 | y_dif = p2.y - p1.y; 50 | if(x_dif < 0) 51 | x_dif = -x_dif; 52 | if(y_dif < 0) 53 | y_dif = -y_dif; 54 | return x_dif + y_dif; 55 | } 56 | 57 | int distanceSquared(const Point_t& p1, const Point_t& p2) 58 | { 59 | int dx = p2.x - p1.x; 60 | int dy = p2.y - p1.y; 61 | 62 | int dx2 = dx * dx; 63 | if (dx2 != 0 && dx2 / dx != dx) 64 | { 65 | throw std::range_error("Integer overflow error for the given points"); 66 | } 67 | 68 | int dy2 = dy * dy; 69 | if (dy2 != 0 && dy2 / dy != dy) 70 | { 71 | throw std::range_error("Integer overflow error for the given points"); 72 | } 73 | 74 | if (dx2 > std::numeric_limits< int >::max() - dy2) 75 | throw std::range_error("Integer overflow error for the given points"); 76 | int d2 = dx2 + dy2; 77 | 78 | return d2; 79 | } 80 | 81 | /** 82 | * Sort vector by the heuristic value of the last element 83 | * @return whether last elem. of first has a larger heuristic value than last elem of second 84 | */ 85 | bool sort_gridNodePath_heuristic_desc(const std::vector &first, const std::vector &second) 86 | { 87 | return (first.back().he > second.back().he); 88 | } 89 | 90 | bool a_star_to_open_space(std::vector > const &grid, gridNode_t init, int cost, 91 | std::vector > &visited, std::list const &open_space, 92 | std::list &pathNodes) 93 | { 94 | uint dx, dy, dx_prev, nRows = grid.size(), nCols = grid[0].size(); 95 | 96 | std::vector > closed(nRows, std::vector(nCols, eNodeOpen)); 97 | // All nodes in the closest list are currently still open 98 | 99 | closed[init.pos.y][init.pos.x] = eNodeVisited; // Of course we have visited the current/initial location 100 | #ifdef DEBUG_PLOT 101 | std::cout << "A*: Marked init " << init << " as eNodeVisited (true)" << std::endl; 102 | printGrid(closed); 103 | #endif 104 | 105 | std::vector > open1(1, std::vector(1, init)); // open1 is a *vector* of paths 106 | 107 | while (true) 108 | { 109 | #ifdef DEBUG_PLOT 110 | std::cout << "A*: open1.size() = " << open1.size() << std::endl; 111 | #endif 112 | if (open1.size() == 0) // If there are no open paths, there's no place to go and we must resign 113 | { 114 | // Empty end_node list and add init as only element 115 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 116 | pathNodes.push_back(init); 117 | return true; // We resign, cannot find a path 118 | } 119 | else 120 | { 121 | // Sort elements from high to low (because sort_gridNodePath_heuristic_desc uses a > b) 122 | std::sort(open1.begin(), open1.end(), sort_gridNodePath_heuristic_desc); 123 | 124 | std::vector nn = open1.back(); // Get the *path* with the lowest heuristic cost 125 | open1.pop_back(); // The last element is no longer open because we use it here, so remove from open list 126 | #ifdef DEBUG_PLOT 127 | std::cout << "A*: Check out path from" << nn.front().pos << " to " << nn.back().pos 128 | << " of length " << nn.size() << std::endl; 129 | #endif 130 | 131 | // Does the path nn end in open space? 132 | if (visited[nn.back().pos.y][nn.back().pos.x] == eNodeOpen) 133 | { 134 | // If so, we found a path to open space 135 | // Copy the path nn to pathNodes so we can report that path (to get to open space) 136 | std::vector::iterator iter; 137 | for (iter = nn.begin(); iter != nn.end(); ++iter) 138 | { 139 | pathNodes.push_back((*iter)); 140 | } 141 | 142 | return false; // We do not resign, we found a path 143 | } 144 | else // Path nn does not lead to open space 145 | { 146 | if (nn.size() > 1) 147 | { 148 | // Create iterator for gridNode_t list and let it point to the last element of nn 149 | std::vector::iterator it = --(nn.end()); 150 | dx = it->pos.x - (it - 1)->pos.x; 151 | dy = it->pos.y - (it - 1)->pos.y; 152 | // TODO(CesarLopez) docs: this seems to cycle through directions 153 | // (notice the shift-by-one between both sides of the =) 154 | dx_prev = dx; 155 | dx = -dy; 156 | dy = dx_prev; 157 | } 158 | else 159 | { 160 | dx = 0; 161 | dy = 1; 162 | } 163 | 164 | // For all nodes surrounding the end of the end of the path nn 165 | for (uint i = 0; i < 4; ++i) 166 | { 167 | Point_t p2 = 168 | { 169 | nn.back().pos.x + dx, 170 | nn.back().pos.y + dy, 171 | }; 172 | 173 | #ifdef DEBUG_PLOT 174 | std::cout << "A*: Look around " << i << " at p2=(" << p2 << std::endl; 175 | #endif 176 | 177 | if (p2.x >= 0 && p2.x < nCols && p2.y >= 0 && p2.y < nRows) // Bounds check, do not sep out of map 178 | { 179 | // If the new node (a neighbor of the end of the path nn) is open, append it to newPath ( = nn) 180 | // and add that to the open1-list of paths. 181 | // Because of the pop_back on open1, what happens is that the path is temporarily 'checked out', 182 | // modified here, and then added back (if the condition above and below holds) 183 | if (closed[p2.y][p2.x] == eNodeOpen && grid[p2.y][p2.x] == eNodeOpen) 184 | { 185 | #ifdef DEBUG_PLOT 186 | std::cout << "A*: p2=" << p2 << " is OPEN" << std::endl; 187 | #endif 188 | std::vector newPath = nn; 189 | // # heuristic has to be designed to prefer a CCW turn 190 | Point_t new_point = { p2.x, p2.y }; 191 | gridNode_t new_node = 192 | { 193 | new_point, // Point: x,y 194 | cost + nn.back().cost, // Cost 195 | cost + nn.back().cost + distanceToClosestPoint(p2, open_space) + i, 196 | // Heuristic (+i so CCW turns are cheaper) 197 | }; 198 | newPath.push_back(new_node); 199 | closed[new_node.pos.y][new_node.pos.x] = eNodeVisited; // New node is now used in a path and thus visited 200 | 201 | #ifdef DEBUG_PLOT 202 | std::cout << "A*: Marked new_node " << new_node << " as eNodeVisited (true)" << std::endl; 203 | std::cout << "A*: Add path from " << newPath.front().pos << " to " << newPath.back().pos 204 | << " of length " << newPath.size() << " to open1" << std::endl; 205 | #endif 206 | open1.push_back(newPath); 207 | } 208 | #ifdef DEBUG_PLOT 209 | else 210 | { 211 | std::cout << "A*: p2=" << p2 << " is not open: " 212 | "closed[" << p2.y << "][" << p2.x << "]=" << closed[p2.y][p2.x] << ", " 213 | "grid[" << p2.y << "][" << p2.x << "]=" << grid[p2.y][p2.x] << std::endl; 214 | } 215 | #endif 216 | } 217 | #ifdef DEBUG_PLOT 218 | else 219 | { 220 | std::cout << "A*: p2=(" << p2.x << ", " << p2.y << ") is out of bounds" << std::endl; 221 | } 222 | #endif 223 | // Cycle around to next neighbor, CCW 224 | dx_prev = dx; 225 | dx = dy; 226 | dy = -dx_prev; 227 | } 228 | } 229 | } 230 | } 231 | } 232 | 233 | void printGrid(std::vector > const& grid, std::vector > const& visited, 234 | std::list const& path) 235 | { 236 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 237 | { 238 | for (uint ix = 0; ix < grid[0].size(); ++ix) 239 | { 240 | if (visited[iy][ix]) 241 | { 242 | if (ix == path.front().x && iy == path.front().y) 243 | { 244 | std::cout << "\033[1;32m▓\033[0m"; // Show starting position in green color 245 | } 246 | else if (ix == path.back().x && iy == path.back().y) 247 | { 248 | std::cout << "\033[1;31m▓\033[0m"; // Show stopping position in red color 249 | } 250 | else if (visited[iy][ix] && grid[iy][ix]) 251 | { 252 | std::cout << "\033[1;33m▓\033[0m"; // Show walls in yellow color 253 | } 254 | else 255 | { 256 | std::cout << "\033[1;36m▓\033[0m"; 257 | } 258 | } 259 | else 260 | { 261 | std::cout << "\033[1;37m▓\033[0m"; 262 | } 263 | } 264 | std::cout << "\n"; 265 | } 266 | } 267 | 268 | void printGrid(std::vector > const& grid, std::vector > const& visited, 269 | std::list const& path, gridNode_t start, gridNode_t end) 270 | { 271 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 272 | { 273 | for (uint ix = 0; ix < grid[0].size(); ++ix) 274 | { 275 | if (visited[iy][ix]) 276 | { 277 | if (ix == start.pos.x && iy == start.pos.y) 278 | { 279 | std::cout << "\033[1;32m▓\033[0m"; // Show starting position in green color 280 | } 281 | else if (ix == end.pos.x && iy == end.pos.y) 282 | { 283 | std::cout << "\033[1;31m▓\033[0m"; // Show stopping position in red color 284 | } 285 | else if (visited[iy][ix] && grid[iy][ix]) 286 | { 287 | std::cout << "\033[1;33m▓\033[0m"; // Show walls in yellow color 288 | } 289 | else 290 | { 291 | std::cout << "\033[1;36m▓\033[0m"; 292 | } 293 | } 294 | else 295 | { 296 | std::cout << "\033[1;37m▓\033[0m"; 297 | } 298 | } 299 | std::cout << "\n"; 300 | } 301 | } 302 | 303 | void printGrid(std::vector > const& grid) 304 | { 305 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 306 | { 307 | for (uint ix = 0; ix < grid[0].size(); ++ix) 308 | { 309 | if (grid[iy][ix]) 310 | { 311 | std::cout << "\033[1;36m▓\033[0m"; 312 | } 313 | else 314 | { 315 | std::cout << "\033[1;37m▓\033[0m"; 316 | } 317 | } 318 | std::cout << "\n"; 319 | } 320 | } 321 | 322 | void printGrid(std::vector > const& grid) 323 | { 324 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 325 | { 326 | for (uint ix = 0; ix < grid[0].size(); ++ix) 327 | { 328 | if (grid[iy][ix]) 329 | { 330 | std::cout << "\033[1;36m▓\033[0m"; 331 | } 332 | else 333 | { 334 | std::cout << "\033[1;37m▓\033[0m"; 335 | } 336 | } 337 | std::cout << "\n"; 338 | } 339 | } 340 | 341 | std::list map_2_goals(std::vector > const& grid, bool value_to_search) 342 | { 343 | std::list goals; 344 | int ix, iy; 345 | uint nRows = grid.size(); 346 | uint nCols = grid[0].size(); 347 | for (iy = 0; iy < nRows; ++(iy)) 348 | { 349 | for (ix = 0; ix < nCols; ++(ix)) 350 | { 351 | if (grid[iy][ix] == value_to_search) 352 | { 353 | Point_t p = { ix, iy }; // x, y 354 | goals.push_back(p); 355 | } 356 | } 357 | } 358 | return goals; 359 | } 360 | -------------------------------------------------------------------------------- /src/aco.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2022] Manchester Metropolitian University" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "full_coverage_path_planner/aco.h" 11 | #include 12 | 13 | // register this planner as a BaseGlobalPlanner plugin 14 | PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::AntColonyOptimization, nav_core::BaseGlobalPlanner) 15 | 16 | namespace full_coverage_path_planner 17 | { 18 | void AntColonyOptimization::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) 19 | { 20 | if (!initialized_) 21 | { 22 | // Create a publisher to visualize the plan 23 | ros::NodeHandle private_nh("~/"); 24 | ros::NodeHandle nh, private_named_nh("~/" + name); 25 | 26 | plan_pub_ = private_named_nh.advertise("plan", 1); 27 | // Try to request the cpp-grid from the cpp_grid map_server 28 | cpp_grid_client_ = nh.serviceClient("static_map"); 29 | 30 | // Define robot radius (radius) parameter 31 | float robot_radius_default = 0.5f; 32 | private_named_nh.param("robot_radius", robot_radius_, robot_radius_default); 33 | // Define tool radius (radius) parameter 34 | float tool_radius_default = 0.5f; 35 | private_named_nh.param("tool_radius", tool_radius_, tool_radius_default); 36 | initialized_ = true; 37 | } 38 | } 39 | 40 | std::list AntColonyOptimization::runACO(std::vector> const &grid, 41 | Point_t &init) 42 | { 43 | // some info on the grid 44 | uint dx, dy, dx_prev, nRows = grid.size(), nCols = grid[0].size(); 45 | // init optimal score is inf 46 | _Float64 globalScoreOptimal = INFINITY; 47 | 48 | // ACO PARAMS 49 | _Float64 evaporation_rate = 0.3; 50 | _Float64 production_rate = 0.6; 51 | _Float64 system_constant = static_cast(rand()) / static_cast(RAND_MAX); 52 | 53 | // 2. initialize pheromone grid 54 | std::vector> og_grid(nRows, std::vector<_Float64>(nCols, 1.0)); 55 | 56 | std::vector> global_pheromone_grid = og_grid; 57 | std::vector> probability_matrix = og_grid; 58 | 59 | ROS_INFO("GRID BUILT"); 60 | 61 | // 1. initialize ants (n=300) 62 | std::list ants; 63 | 64 | Point_t start = {init.x, init.y}; 65 | 66 | int velocity = 8; 67 | for (int i = 0; i < 1000; ++i) 68 | { 69 | if(i < 100) 70 | velocity = 8; 71 | else if(i >= 10 && i < 20) 72 | velocity = 7; 73 | else if(i >= 20 && i < 30) 74 | velocity = 6; 75 | else if(i >= 30 && i < 40) 76 | velocity = 5; 77 | else if(i >= 40 && i < 60) 78 | velocity = 4; 79 | else if (i >= 60 && i < 70) 80 | velocity = 3; 81 | else if (i >= 70 && i < 80) 82 | velocity = 2; 83 | else 84 | velocity = 1; 85 | 86 | std::vector> personal_pheromone_grid = og_grid; 87 | std::vector> personal_scored_grid = og_grid; 88 | ants.push_back(Ant({start, 0, 0}, personal_pheromone_grid, probability_matrix, velocity)); 89 | } 90 | 91 | // we will initialize the optimal ant as the first ant in the list to begin. 92 | 93 | Ant globalOptimalAnt = ants.front(); 94 | std::list globalOptimalPath; 95 | 96 | ROS_INFO("ALL ANTS BUILT"); 97 | // foreach ant 98 | 99 | std::list ants_completed_tour; 100 | 101 | int optimalAntPos = 0; 102 | _Float64 scoreOptimal = INFINITY; 103 | int antCount = 0; 104 | int globalMultipassCounter; 105 | for (int optimizationCount = 0; optimizationCount < 10; optimizationCount++) 106 | { 107 | ROS_INFO("RUN %i", optimizationCount); 108 | std::list optimalAntPath; 109 | for (Ant ant : ants) 110 | { 111 | auto visited(grid); 112 | // make sure ant is in init location 113 | ant.move({{init.x, init.y}, 0, 0}, false); 114 | // first location is visited 115 | visited[init.y][init.x] = eNodeVisited; 116 | // calculate probability matrix for ant 117 | ant.calculateProbabilityMatrix(global_pheromone_grid); 118 | 119 | std::list goals = map_2_goals(visited, eNodeOpen); 120 | while (!goals.empty()) 121 | { 122 | int orig_x = ant.current_location.pos.x; 123 | int orig_y = ant.current_location.pos.y; 124 | 125 | // ant.ant_velocity = rand() % 5; // random velocity between 1..8 126 | 127 | std::list possible_movements = ant.getPossibleMovements(visited, orig_x, orig_y); 128 | gridNode_t new_location; 129 | if (possible_movements.size() == 0) // ant has nowhere to go ! :( 130 | { 131 | // A* to open path? 132 | if (ant.resolveDeadlock(visited, goals, grid)) 133 | { 134 | // A* cannot find a path to an open space. It's maybe solved? 135 | break; 136 | } 137 | else 138 | { 139 | // A* took the ant to a new path. Go to next loop iteration 140 | ant.current_location = ant.current_path.back(); 141 | new_location = ant.current_path.back(); 142 | } 143 | } 144 | else 145 | { 146 | // ant can make some movements, follow on creating probability distro from pheromones. 147 | // 70% of the time, ant follows best path. 148 | float will_follow_best_path_rng = static_cast(rand()) / static_cast(RAND_MAX); 149 | if (will_follow_best_path_rng > 0.3) 150 | { 151 | gridNode_t bestPoint = ant.getBestMovement(possible_movements, global_pheromone_grid); 152 | new_location = bestPoint; 153 | } 154 | else 155 | { 156 | // need proba distrobution 157 | std::list probabiltyDistro = ant.generateProbabilityDistro(possible_movements, global_pheromone_grid); 158 | // find next tile from generated probability distro 159 | _Float64 probaSum = 0; 160 | float proba_rng = static_cast(rand()) / static_cast(RAND_MAX); 161 | for (Pos_proba pp : probabiltyDistro) 162 | { 163 | probaSum += pp.proba; 164 | if (probaSum > proba_rng) 165 | { 166 | new_location = pp.pos; 167 | break; 168 | } 169 | } 170 | } 171 | } 172 | // move ant 173 | visited[new_location.pos.y][new_location.pos.x] = eNodeVisited; 174 | ant.move(new_location, true); 175 | 176 | // produce pheromones 177 | _Float64 pheromones = ant.producePheromones(evaporation_rate, production_rate, system_constant); 178 | global_pheromone_grid[new_location.pos.y][new_location.pos.x] = evaporation_rate * (pheromones + (system_constant * global_pheromone_grid[new_location.pos.y][new_location.pos.x])); 179 | ant.visited_counter++; 180 | 181 | // move ant extra according to velocity 182 | int nx = new_location.pos.x; 183 | int ny = new_location.pos.y; 184 | 185 | for (int moved = 1; moved < ant.ant_velocity; moved++) 186 | { 187 | if (ant.direction == NORTH) 188 | { 189 | ny++; 190 | } 191 | if (ant.direction == EAST) 192 | { 193 | nx++; 194 | } 195 | if (ant.direction == SOUTH) 196 | { 197 | ny--; 198 | } 199 | if (ant.direction == WEST) 200 | { 201 | nx--; 202 | } 203 | 204 | gridNode_t nPos = {{nx, ny}, 0, 0}; 205 | 206 | if (visited[ny][nx] == eNodeOpen) 207 | { 208 | ant.move(nPos, true); 209 | ant.visited_counter++; 210 | visited[ny][nx] = eNodeVisited; 211 | _Float64 pheromones = ant.producePheromones(evaporation_rate, production_rate, system_constant); 212 | global_pheromone_grid[new_location.pos.y][new_location.pos.x] = evaporation_rate * (pheromones + (system_constant * global_pheromone_grid[new_location.pos.y][new_location.pos.x])); 213 | } 214 | else 215 | { 216 | break; 217 | } 218 | } 219 | 220 | // regenerate remaining goalpoints 221 | goals = map_2_goals(visited, eNodeOpen); 222 | } 223 | 224 | // move ant to it's initial location 225 | visited[init.y][init.x] = eNodeOpen; 226 | goals.push_back({init.x, init.y}); 227 | ant.resolveDeadlock(visited, goals, grid); 228 | // recalculate some metrics 229 | ant.multiple_pass_counter = 0; 230 | std::list closedSet; 231 | int pathSize = ant.current_path.size(); 232 | auto test_grid(grid); 233 | for (gridNode_t nodeOnPath : ant.current_path) 234 | { 235 | if (test_grid[nodeOnPath.pos.y][nodeOnPath.pos.x] == eNodeVisited) 236 | { 237 | ant.multiple_pass_counter++; 238 | } 239 | else 240 | { 241 | test_grid[nodeOnPath.pos.y][nodeOnPath.pos.x] = eNodeVisited; 242 | } 243 | } 244 | ROS_INFO("ant %i solved", antCount); 245 | _Float64 ant_score_final = goals.size() * 1000 + ant.multiple_pass_counter; 246 | ROS_INFO("score, %f, multipass %i", ant_score_final, ant.multiple_pass_counter); 247 | 248 | if (ant_score_final < scoreOptimal) 249 | { 250 | optimalAntPos = antCount; 251 | optimalAntPath = ant.current_path; 252 | scoreOptimal = ant_score_final; 253 | if (ant_score_final < globalScoreOptimal) 254 | { 255 | globalOptimalAnt = ant; 256 | globalOptimalPath = ant.current_path; 257 | globalScoreOptimal = ant_score_final; 258 | globalMultipassCounter = ant.multiple_pass_counter; 259 | } 260 | } 261 | antCount++; 262 | 263 | // reset the path information for next run 264 | ant.current_path.clear(); 265 | } 266 | 267 | // add more pheromones to best ants trail. 268 | std::list::iterator antIt = ants.begin(); 269 | std::advance(antIt, optimalAntPos); 270 | 271 | for (gridNode_t it : optimalAntPath) 272 | { 273 | _Float64 new_pheromone_value_for_best_ant = 274 | 50 * global_pheromone_grid[it.pos.y][it.pos.x]; // make it so 50x ants took the pheromones on the best ants personal trail. - this is to simulate many ants taking the shorter route, which doesn't actually happen in the fcpp problem. 275 | antIt->personal_pheromone_grid[it.pos.y][it.pos.x] = new_pheromone_value_for_best_ant; 276 | // global trail updating formula 277 | global_pheromone_grid[it.pos.y][it.pos.x] = new_pheromone_value_for_best_ant; 278 | } 279 | } 280 | 281 | /// end 282 | ROS_INFO("OPTIMAL ANT PATH"); 283 | ROS_INFO("PATH LENGTH: %li", globalOptimalPath.size()); 284 | ROS_INFO("SCORE: %f", globalScoreOptimal); 285 | ROS_INFO("MULTIPASS: %i", globalMultipassCounter); 286 | 287 | ROS_INFO("building path for planner"); 288 | 289 | std::list fullPath; 290 | std::list::iterator it; 291 | for (it = globalOptimalPath.begin(); it != globalOptimalPath.end(); it++) 292 | { 293 | 294 | fullPath.push_back(it->pos); 295 | } 296 | 297 | return fullPath; 298 | } 299 | 300 | bool AntColonyOptimization::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 301 | std::vector &plan) 302 | { 303 | if (!initialized_) 304 | { 305 | ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 306 | return false; 307 | } 308 | else 309 | { 310 | ROS_INFO("Initialized!"); 311 | } 312 | 313 | clock_t begin = clock(); 314 | Point_t startPoint; 315 | 316 | /********************** Get grid from server **********************/ 317 | std::vector> grid; 318 | nav_msgs::GetMap grid_req_srv; 319 | ROS_INFO("Requesting grid!!"); 320 | if (!cpp_grid_client_.call(grid_req_srv)) 321 | { 322 | ROS_ERROR("Could not retrieve grid from map_server"); 323 | return false; 324 | } 325 | 326 | if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) 327 | { 328 | ROS_ERROR("Could not parse retrieved grid"); 329 | return false; 330 | } 331 | 332 | #ifdef DEBUG_PLOT 333 | ROS_INFO("Start grid is:"); 334 | std::list printPath; 335 | printPath.push_back(startPoint); 336 | printGrid(grid, grid, printPath); 337 | #endif 338 | 339 | std::list goalPoints = runACO(grid, startPoint); 340 | ROS_INFO("naive cpp completed!"); 341 | ROS_INFO("Converting path to plan"); 342 | 343 | parsePointlist2Plan(start, goalPoints, plan); 344 | // Print some metrics: 345 | spiral_cpp_metrics_.accessible_counter = spiral_cpp_metrics_.visited_counter - spiral_cpp_metrics_.multiple_pass_counter; 346 | spiral_cpp_metrics_.total_area_covered = (4.0 * tool_radius_ * tool_radius_) * spiral_cpp_metrics_.accessible_counter; 347 | ROS_INFO("Total visited: %d", spiral_cpp_metrics_.visited_counter); 348 | ROS_INFO("Total re-visited: %d", spiral_cpp_metrics_.multiple_pass_counter); 349 | ROS_INFO("Total accessible cells: %d", spiral_cpp_metrics_.accessible_counter); 350 | ROS_INFO("Total accessible area: %f", spiral_cpp_metrics_.total_area_covered); 351 | 352 | // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept 353 | // (also controlled by planner_frequency parameter in move_base namespace) 354 | 355 | ROS_INFO("Publishing plan!"); 356 | publishPlan(plan); 357 | ROS_INFO("Plan published!"); 358 | ROS_DEBUG("Plan published"); 359 | 360 | clock_t end = clock(); 361 | double elapsed_secs = static_cast(end - begin) / CLOCKS_PER_SEC; 362 | std::cout << "elapsed time: " << elapsed_secs << "\n"; 363 | 364 | return true; 365 | } 366 | } -------------------------------------------------------------------------------- /test/src/test_common.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // Created by nobleo on 25-9-18. 4 | // 5 | 6 | /* 7 | * Run tests for all of the common function except the print* functions that do not return anything testable 8 | * Most important here is the conversion function and a variant of A*. Each test is explained below 9 | * 10 | */ 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | /** 21 | * DistanceSquared uses euclidian distance except for the expensive sqrt-call: returns dx^2+dy^2. 22 | */ 23 | TEST(TestDistanceSquared, testDistanceSquared) 24 | { 25 | // 1^2 + 1^2 = 1 + 1 = 2 26 | ASSERT_EQ(2, distanceSquared({0, 0}, {1, 1})); // NOLINT 27 | 28 | // 10^2 + 10^2 = 100 + 100 = 200 29 | ASSERT_EQ(200, distanceSquared({0, 0}, {10, 10})); // NOLINT 30 | 31 | /* The function uses plain integers. Squaring and then adding them can go out of range of int. 32 | * In that case: throw an exception, don't crash 33 | */ 34 | ASSERT_THROW(distanceSquared({0, 0}, {100000, 100000}), std::range_error); // NOLINT 35 | 36 | /* Points at equal distance in direct directions should have same value 37 | */ 38 | ASSERT_EQ(distanceSquared({0, 0}, {11, 10}), // NOLINT 39 | distanceSquared({0, 0}, {10, 11})); // NOLINT 40 | 41 | /* The function is used mostly to order points by distance, so the actual value doesn't matter. 42 | * The only property that is important is that points further away should have a larger value than those close by 43 | */ 44 | ASSERT_LE(distanceSquared({0, 0}, {10, 10}), // NOLINT 45 | distanceSquared({0, 0}, {11, 11})); // NOLINT 46 | } 47 | 48 | /* 49 | * Test basics of distanceToClosestPoint: 50 | * - does it return the only point if there is one only 51 | * - if we add another further away, is the first point still returned 52 | */ 53 | TEST(TestDistanceToClosestPoint, testDistanceToOnlyPoint) 54 | { 55 | Point_t poi = {0, 0}; // NOLINT 56 | std::list goals; 57 | goals.push_back({1, 1}); // NOLINT 58 | 59 | // There is only 1 point, at 1,1, so the (squared) distance is 2 60 | ASSERT_EQ(2, distanceToClosestPoint(poi, goals)); 61 | 62 | goals.push_back({2, 2}); // NOLINT // We add a point that is further away, so the first point is still closest 63 | ASSERT_EQ(2, distanceToClosestPoint(poi, goals)); 64 | } 65 | 66 | /* 67 | * Add several points, 2 at same distance 68 | */ 69 | TEST(TestDistanceToClosestPoint, testDistanceToEqualDistancePoints) 70 | { 71 | Point_t poi = {0, 0}; // NOLINT 72 | std::list goals; 73 | goals.push_back({0, 1}); // NOLINT // closest, d=1 74 | goals.push_back({1, 0}); // NOLINT // closest, d=1 75 | goals.push_back({1, 1}); // NOLINT 76 | goals.push_back({2, 2}); // NOLINT 77 | 78 | ASSERT_EQ(1, distanceToClosestPoint(poi, goals)); 79 | 80 | goals.push_back({0, 0}); // NOLINT // new closest, d=0 81 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 82 | } 83 | 84 | /* 85 | * distanceToClosestPoint cannot deal with dimensions larger than 2^16 because it does a square of that value 86 | * Function should not crash if one distance is <2^16 but raise an exception if we go over 87 | */ 88 | TEST(TestDistanceToClosestPoint, testDistanceAtIntLimits) 89 | { 90 | Point_t poi = {0, 0}; // NOLINT 91 | std::list goals; 92 | for (int i = 0; i < 32768; ++i) 93 | { 94 | goals.push_back({i, i}); // NOLINT 95 | } 96 | 97 | ASSERT_NO_THROW(distanceToClosestPoint(poi, goals)); // OK for small enough dimensions 98 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 99 | 100 | for (int i = 32768; i < 100000; ++i) 101 | { 102 | goals.push_back({i, i}); // NOLINT 103 | } 104 | 105 | // Squaring and adding 100000 is too much for an int 106 | ASSERT_THROW(distanceToClosestPoint(poi, goals), std::range_error); // Must throw for large enough dimensions 107 | } 108 | 109 | /* 110 | * Same as testDistanceAtIntLimits but with negative numbers 111 | */ 112 | TEST(TestDistanceToClosestPoint, testDistanceAtIntNegativeLimits) 113 | { 114 | Point_t poi = {0, 0}; // NOLINT 115 | std::list goals; 116 | for (int i = 0; i < 32768; ++i) 117 | { 118 | goals.push_back({-i, -i}); // NOLINT 119 | } 120 | 121 | ASSERT_NO_THROW(distanceToClosestPoint(poi, goals)); 122 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 123 | 124 | for (int i = 32768; i < 100000; ++i) 125 | { 126 | goals.push_back({-i, -i}); // NOLINT 127 | } 128 | 129 | // Squaring and adding 100000 is too much for an int 130 | ASSERT_THROW(distanceToClosestPoint(poi, goals), std::range_error); 131 | } 132 | 133 | /* 134 | * Test points in a block a bit further away 135 | */ 136 | TEST(TestDistanceToClosestPoint, testDistanceToBlock) 137 | { 138 | Point_t poi = {0, 0}; // NOLINT 139 | std::list goals; 140 | goals.push_back({10, 10}); // NOLINT // closest, d^2=200 141 | goals.push_back({11, 10}); // NOLINT 142 | goals.push_back({10, 11}); // NOLINT 143 | goals.push_back({12, 11}); // NOLINT 144 | 145 | ASSERT_EQ(200, distanceToClosestPoint(poi, goals)); 146 | 147 | goals.push_back({0, 0}); // NOLINT // new closest, d=0 148 | ASSERT_EQ(0, distanceToClosestPoint(poi, goals)); 149 | } 150 | 151 | /* 152 | * Test points in a stroke a bit further away 153 | */ 154 | TEST(TestDistanceToClosestPoint, testDistanceToDiagonal) 155 | { 156 | Point_t poi = {100, 100}; // NOLINT 157 | std::list goals; 158 | for (int i = 0; i < 10; ++i) 159 | { 160 | for (int j = 0; j < 10; ++j) 161 | { 162 | goals.push_back({i, j}); // NOLINT 163 | } 164 | } 165 | 166 | // Closest point is 9,9, so distance is 91^2 + 91^2 167 | ASSERT_EQ(16562, distanceToClosestPoint(poi, goals)); 168 | } 169 | 170 | /* 171 | * Test a test utility function to make grids. 172 | * Grids must all have specified size but now allow access beyond those limits. 173 | * Users of the resulting test grid are also not allowed to access beyond the limits 174 | * and should crash if they do for proper testing. 175 | */ 176 | TEST(TestMakeTestGrid, testDimensions) 177 | { 178 | std::vector > grid = makeTestGrid(3, 4); 179 | ASSERT_EQ(4, grid.size()); 180 | ASSERT_EQ(3, grid.at(0).size()); 181 | ASSERT_EQ(3, grid.at(1).size()); 182 | ASSERT_EQ(3, grid.at(2).size()); 183 | ASSERT_EQ(3, grid.at(3).size()); 184 | ASSERT_ANY_THROW(grid.at(3).at(3)); // Only 3 items in X direction (horizontal) so no index 3 185 | ASSERT_ANY_THROW(grid.at(4).size()); // Only 4 items in Y direction (vertical) so no index 4 186 | } 187 | 188 | /* 189 | * Test that if there is a NxN map with only a single element, only that single element is returned 190 | */ 191 | TEST(TestMap_2_goals, testFindSingle) 192 | { 193 | /* Map will be 3x3 with only the middle value set to true, the others being false 194 | * 195 | * [ 0 0 0 ] 196 | * [ 0 1 0 ] 197 | * [ 0 0 0 ] 198 | */ 199 | std::vector > grid = makeTestGrid(3, 3); 200 | grid[1][1] = true; 201 | std::list goals; 202 | goals = map_2_goals(grid, true); 203 | ASSERT_EQ(1, goals.size()); 204 | 205 | Point_t center = {1, 1}; // NOLINT 206 | ASSERT_EQ(center.x, goals.front().x); 207 | ASSERT_EQ(center.y, goals.front().y); 208 | } 209 | 210 | /* 211 | * Test that if there is a 3x3 map with 3 elements, that those elements and no more are returned 212 | */ 213 | TEST(TestMap_2_goals, testNumberOfGoals) 214 | { 215 | /* Map: 216 | * [ 1 0 0 ] 217 | * [ 0 1 0 ] 218 | * [ 0 0 1 ] 219 | */ 220 | std::vector > grid = makeTestGrid(3, 3); 221 | grid[0][0] = true; 222 | grid[1][1] = true; 223 | grid[2][2] = true; 224 | std::list goals; 225 | goals = map_2_goals(grid, true); 226 | std::vector goalVector = std::vector(goals.begin(), goals.end()); 227 | 228 | // We only set 3 points to 1, so we should only find those 3 229 | ASSERT_EQ(3, goals.size()); 230 | 231 | // And specifically those 3, not anything else 232 | Point_t corner0 = {0, 0}; // NOLINT 233 | Point_t center = {1, 1}; // NOLINT 234 | Point_t corner2 = {2, 2}; // NOLINT 235 | ASSERT_EQ(corner0, goalVector.at(0)); 236 | ASSERT_EQ(center, goalVector.at(1)); 237 | ASSERT_EQ(corner2, goalVector.at(2)); 238 | } 239 | 240 | /* 241 | * map_2_goals can look for both true and false cells. 242 | * In either case, we should return the correct number of goals 243 | */ 244 | TEST(TestMap_2_goals, testInvertedMap) 245 | { 246 | /* Map: 247 | * [ 1 1 1 ] 248 | * [ 1 0 1 ] 249 | * [ 1 1 1 ] 250 | */ 251 | std::vector > grid = makeTestGrid(3, 3, true); 252 | grid[1][1] = false; 253 | 254 | ASSERT_EQ(8, map_2_goals(grid, true).size()); // There are 8 true values 255 | ASSERT_EQ(1, map_2_goals(grid, false).size()); // There is only 1 false value 256 | } 257 | 258 | /* 259 | * If we use a non-square grid, do we use the correct order of indexing? 260 | * Coordinates use x,y and y indexes over rows, x over indices in that row 261 | */ 262 | TEST(TestMap_2_goals, testCoordinateOrder) 263 | { 264 | /* Map is 3x4 265 | * [ 0 0 0 ] 266 | * [ 0 0 0 ] 267 | * [ 0 0 0 ] 268 | * [ 0 0 1 ] 269 | * 270 | */ 271 | std::vector > grid = makeTestGrid(3, 4, false); 272 | grid.at(3).at(2) = true; 273 | std::list goals; 274 | 275 | goals = map_2_goals(grid, true); 276 | ASSERT_EQ(1, goals.size()); 277 | 278 | Point_t corner0 = {2, 3}; // NOLINT 279 | ASSERT_EQ(corner0.x, goals.front().x); 280 | ASSERT_EQ(corner0.y, goals.front().y); 281 | } 282 | 283 | /* LEGENDA 284 | * Note: in tests for the A* path finding algorithm, use this legend for the maps: 285 | * s: start 286 | * p: path node 287 | * v: visited 288 | * 1: obstacle / occupied 289 | * 0: open node / unoccupied 290 | */ 291 | 292 | /* 293 | * On an empty map, find a route to open space should be as simple as stepping to the next position 294 | * (when the current position is already visited and thus not OK) 295 | */ 296 | TEST(TestAStarToOpenSpace, testEmptyMap) 297 | { 298 | /* 299 | * [s] We start from here, so this is visited 300 | * [0] And this is still open, so we should go here to find open space 301 | * [0] 302 | * [0] 303 | */ 304 | std::vector > grid = makeTestGrid(1, 4, false); 305 | std::vector > visited = makeTestGrid(1, 4, false); 306 | std::list goals; 307 | goals.push_back({0, 3}); // NOLINT 308 | 309 | visited[0][0] = true; 310 | gridNode_t start; 311 | start.pos = {0, 0}; // NOLINT 312 | start.cost = 1; 313 | start.he = 0; 314 | 315 | std::list pathNodes; 316 | 317 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 318 | start, // Start 319 | 1, // Cost of traversing a node 320 | visited, // Visited nodes, of which there are none yet 321 | goals, 322 | pathNodes); 323 | /* 324 | * [p] We came from here 325 | * [p] and this is the first unvisited, non-obstacle cell so we step here 326 | * [0] 327 | * [0] 328 | */ 329 | 330 | std::vector pathNodesVector = std::vector(pathNodes.begin(), pathNodes.end()); 331 | ASSERT_EQ(false, resign); 332 | 333 | // First element is that initial node 334 | ASSERT_EQ(0, pathNodesVector.at(0).pos.x); 335 | ASSERT_EQ(0, pathNodesVector.at(0).pos.y); 336 | 337 | // First step we take reaches free space 338 | ASSERT_EQ(0, pathNodesVector.at(1).pos.x); 339 | ASSERT_EQ(1, pathNodesVector.at(1).pos.y); 340 | 341 | // 2 nodes: start and end 342 | ASSERT_EQ(2, pathNodes.size()); 343 | } 344 | 345 | /* 346 | * Small extension of testEmptyMap: there is one more cell already visited 347 | * That mean the path is thus from start, over the one visited cell to a new unvisited cell, so length 3 348 | */ 349 | TEST(TestAStarToOpenSpace, testSingleVisitedCellMap) 350 | { 351 | /* 352 | * [s] 353 | * [v] 354 | * [0] 355 | * [0] 356 | */ 357 | 358 | std::vector > grid = makeTestGrid(1, 4, false); 359 | std::vector > visited = makeTestGrid(1, 4, false); 360 | std::list goals = map_2_goals(grid, true); 361 | 362 | visited[0][0] = true; 363 | visited[1][0] = true; 364 | gridNode_t start; 365 | start.pos = {0, 0}; // NOLINT 366 | start.cost = 1; 367 | start.he = 0; 368 | 369 | std::list pathNodes; 370 | 371 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 372 | start, // Start 373 | 1, // Cost of traversing a node 374 | visited, // Visited nodes, of which there are none yet 375 | goals, 376 | pathNodes); 377 | /* 378 | * [p] 379 | * [p] 380 | * [p] 381 | * [0] 382 | */ 383 | std::vector pathNodesVector = std::vector(pathNodes.begin(), pathNodes.end()); 384 | ASSERT_EQ(false, resign); 385 | 386 | ASSERT_EQ(0, pathNodesVector.at(0).pos.x); 387 | ASSERT_EQ(0, pathNodesVector.at(0).pos.y); 388 | 389 | ASSERT_EQ(0, pathNodesVector.at(1).pos.x); 390 | ASSERT_EQ(1, pathNodesVector.at(1).pos.y); 391 | 392 | ASSERT_EQ(0, pathNodesVector.at(2).pos.x); 393 | ASSERT_EQ(2, pathNodesVector.at(2).pos.y); 394 | 395 | ASSERT_EQ(3, pathNodes.size()); 396 | } 397 | 398 | /* 399 | * When we start in the corner and have somehow visited the direct neighbors, the shortest path should involve 3 points 400 | */ 401 | TEST(TestAStarToOpenSpace, testSingleMulticellVisitedMap) 402 | { 403 | /* 404 | * [s v 0 0] 405 | * [v v 0 0] 406 | * [0 0 0 0] 407 | * [0 0 0 0] 408 | */ 409 | std::vector > grid = makeTestGrid(4, 4, false); 410 | std::vector > visited = makeTestGrid(4, 4, false); 411 | std::list goals = map_2_goals(grid, true); 412 | 413 | visited[0][0] = true; 414 | visited[1][0] = true; 415 | visited[0][1] = true; 416 | visited[1][1] = true; 417 | 418 | gridNode_t start; 419 | start.pos = {0, 0}; // NOLINT 420 | start.cost = 1; 421 | start.he = 0; 422 | 423 | std::list pathNodes; 424 | 425 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 426 | start, // Start 427 | 1, // Cost of traversing a node 428 | visited, // Visited nodes, of which there are none yet 429 | goals, 430 | pathNodes); 431 | /* Several paths possible, but each covers 3 nodes, e.g. 432 | * [p p p 0] 433 | * [v v 0 0] 434 | * [0 0 0 0] 435 | * [0 0 0 0] 436 | */ 437 | std::vector pathNodesVector = std::vector(pathNodes.begin(), pathNodes.end()); 438 | ASSERT_EQ(false, resign); 439 | ASSERT_EQ(3, pathNodes.size()); 440 | } 441 | 442 | /* 443 | * In a map with 2 walls and everything between those wall already visited, test that the path has the expected length 444 | */ 445 | TEST(TestAStarToOpenSpace, testMazeMap) 446 | { 447 | /* 448 | * [s v v v] 449 | * [1 1 1 v] 450 | * [0 v v v] goal is the single 0 in this map 451 | * [1 1 1 1] 452 | */ 453 | std::vector > grid = makeTestGrid(4, 4, false); 454 | grid[1][0] = 1; 455 | grid[1][1] = 1; 456 | grid[1][2] = 1; 457 | grid[3][0] = 1; 458 | grid[3][1] = 1; 459 | grid[3][2] = 1; 460 | grid[3][3] = 1; 461 | std::vector > visited = makeTestGrid(4, 4, false); 462 | std::list goals = map_2_goals(grid, true); 463 | 464 | visited[0][0] = 1; 465 | visited[0][1] = 1; 466 | visited[0][2] = 1; 467 | visited[0][3] = 1; 468 | visited[1][3] = 1; 469 | visited[2][3] = 1; 470 | visited[2][2] = 1; 471 | visited[2][1] = 1; 472 | 473 | gridNode_t start; 474 | start.pos = {0, 0}; // NOLINT 475 | start.cost = 1; 476 | start.he = 0; 477 | 478 | std::list pathNodes; 479 | 480 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 481 | start, // Start 482 | 1, // Cost of traversing a node 483 | visited, // Visited nodes, of which there are none yet 484 | goals, 485 | pathNodes); 486 | /* 487 | * [p p p p] 488 | * [1 1 1 p] 489 | * [p p p p] 490 | * [1 1 1 1] 491 | */ 492 | ASSERT_EQ(false, resign); 493 | ASSERT_EQ(9, pathNodes.size()); 494 | } 495 | 496 | /* 497 | * Extension of testMazeMap, but the bottom wall has a hole that is also not yet visited. 498 | * That is the closest open space, so check that the found path goes there. 499 | */ 500 | TEST(TestAStarToOpenSpace, testMazeWithHoleMap) 501 | { 502 | /* There are 2 open spaces in this test, the one at 3,3 is closest 503 | * [s v v v] 504 | * [1 1 1 v] 505 | * [0 v v v] 506 | * [1 1 1 0] 507 | */ 508 | std::vector > grid = makeTestGrid(4, 4, false); 509 | grid[1][0] = 1; 510 | grid[1][1] = 1; 511 | grid[1][2] = 1; 512 | grid[3][0] = 1; 513 | grid[3][1] = 1; 514 | grid[3][2] = 1; 515 | std::vector > visited = makeTestGrid(4, 4, false); 516 | std::list goals = map_2_goals(grid, true); 517 | 518 | visited[0][0] = 1; 519 | visited[0][1] = 1; 520 | visited[0][2] = 1; 521 | visited[0][3] = 1; 522 | visited[1][3] = 1; 523 | visited[2][3] = 1; 524 | visited[2][2] = 1; 525 | visited[2][1] = 1; 526 | 527 | gridNode_t start; 528 | start.pos = {0, 0}; // NOLINT 529 | start.cost = 1; 530 | start.he = 0; 531 | 532 | std::list pathNodes; 533 | 534 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 535 | start, // Start 536 | 1, // Cost of traversing a node 537 | visited, // Visited nodes, of which there are none yet 538 | goals, 539 | pathNodes); 540 | /* 541 | * [p p p p] 542 | * [1 1 1 p] 543 | * [0 v v p] // These 2 visited nodes but unconnected nodes cannot happen in reality but fine for this test 544 | * [1 1 1 p] 545 | */ 546 | ASSERT_EQ(false, resign); 547 | ASSERT_EQ(7, pathNodes.size()); 548 | } 549 | 550 | /* 551 | * When we start in the corner and have obstacles around us, check that we can indeed not find a path 552 | */ 553 | TEST(TestAStarToOpenSpace, testBlockedMap) 554 | { 555 | /* 556 | * [s 1 0 0] 557 | * [1 1 0 0] 558 | * [0 0 0 0] 559 | * [0 0 0 0] 560 | */ 561 | std::vector > grid = makeTestGrid(4, 4, false); 562 | std::vector > visited = makeTestGrid(4, 4, false); 563 | 564 | visited[0][0] = true; 565 | grid[1][0] = true; 566 | grid[0][1] = true; 567 | grid[1][1] = true; 568 | std::list goals = map_2_goals(grid, true); 569 | 570 | gridNode_t start; 571 | start.pos = {0, 0}; // NOLINT 572 | start.cost = 1; 573 | start.he = 0; 574 | 575 | std::list pathNodes; 576 | 577 | bool resign = a_star_to_open_space(grid, // map to traverse, all empty 578 | start, // Start 579 | 1, // Cost of traversing a node 580 | visited, // Visited nodes, of which there are none yet 581 | goals, 582 | pathNodes); 583 | // No path possible so we should resign 584 | ASSERT_EQ(true, resign); 585 | ASSERT_EQ(1, pathNodes.size()); // Only the cell we start at: 586 | ASSERT_EQ(start.pos, pathNodes.front().pos); 587 | } 588 | // Run all the tests that were declared with TEST() 589 | int main(int argc, char **argv) 590 | { 591 | testing::InitGoogleTest(&argc, argv); 592 | return RUN_ALL_TESTS(); 593 | } 594 | -------------------------------------------------------------------------------- /test/src/test_spiral_stc.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | // Created by nobleo on 27-9-18. 5 | // 6 | 7 | /* 8 | * Full coverage simply requires all reachable nodes to be visited. 9 | * We check this by counting the number of unique elements in the path. 10 | * By putting the path nodes in a set, we are left with only the unique elements 11 | * and then we can count how big that set is (i.e. the cardinality of the set of path nodes) 12 | */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | cv::Mat drawMap(std::vector > const& grid); 31 | 32 | cv::Mat drawPath(const cv::Mat &mapImg, 33 | const cv::Mat &pathImg, 34 | const Point_t &start, 35 | std::list &path); 36 | 37 | /* 38 | * On a map with nothing on it, spiral_stc should cover all the nodes of the map 39 | */ 40 | TEST(TestSpiralStc, testFillEmptyMap) 41 | { 42 | std::vector > grid = makeTestGrid(4, 4, false); 43 | 44 | Point_t start = {0, 0}; 45 | int multiple_pass_counter, visited_counter; 46 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 47 | start, 48 | multiple_pass_counter, 49 | visited_counter); 50 | 51 | ASSERT_EQ(4 * 4, path.size()); // All nodes of the 4x4 map are covered 52 | } 53 | 54 | /* 55 | * On a map with a single obstacle, all the other nodes should still be visited. 56 | */ 57 | TEST(TestSpiralStc, testFillMapWithOneObstacle) 58 | { 59 | /* 60 | * [s 0 0 0] 61 | * [0 0 0 0] 62 | * [0 0 1 0] 63 | * [0 0 0 0] 64 | */ 65 | std::vector > grid = makeTestGrid(4, 4, false); 66 | grid[2][2] = 1; 67 | 68 | Point_t start = {0, 0}; 69 | int multiple_pass_counter, visited_counter; 70 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 71 | start, 72 | multiple_pass_counter, 73 | visited_counter); 74 | 75 | // By Adding the nodes of the path to the set, we only retain the unique elements 76 | std::set pathSet(path.begin(), path.end()); 77 | 78 | // Because the area is 4*4 but with 1 obstacle, coverage all reachable nodes should over 4*4 - 1 = 15 nodes 79 | ASSERT_EQ((4 * 4) - 1, pathSet.size()); 80 | } 81 | 82 | /* 83 | * In a map with 2 obstacles, still the complete map should be covered except for those 2 obstacles 84 | */ 85 | TEST(TestSpiralStc, testFillMapWith2Obstacles) 86 | { 87 | /* 88 | * [s 0 0 0] 89 | * [0 0 0 0] 90 | * [0 0 1 0] 91 | * [0 0 0 1] 92 | */ 93 | std::vector > grid = makeTestGrid(4, 4, false); 94 | grid[2][2] = 1; 95 | grid[3][3] = 1; 96 | 97 | Point_t start = {0, 0}; 98 | int multiple_pass_counter, visited_counter; 99 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 100 | start, 101 | multiple_pass_counter, 102 | visited_counter); 103 | 104 | // By Adding the nodes of the path to the set, we only retain the unique elements 105 | std::set pathSet(path.begin(), path.end()); 106 | 107 | // Because the area is 4*4 but with 1 obstacle, coverage all reachable nodes should over 4*4 - 2 = 14 nodes 108 | ASSERT_EQ((4 * 4) - 2, pathSet.size()); 109 | } 110 | 111 | /* 112 | * On a 4x4 map where the opposite right half of the map is blocked, we can cover only the 4x2 reachable nodes 113 | */ 114 | TEST(TestSpiralStc, testFillMapWithHalfBlocked) 115 | { 116 | /* 117 | * [s 0 1 0] 118 | * [0 0 1 0] 119 | * [0 0 1 0] 120 | * [0 0 1 0] 121 | */ 122 | std::vector > grid = makeTestGrid(4, 4, false); 123 | grid[0][2] = 1; 124 | grid[1][2] = 1; 125 | grid[2][2] = 1; 126 | grid[3][2] = 1; 127 | 128 | Point_t start = {0, 0}; 129 | int multiple_pass_counter, visited_counter; 130 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 131 | start, 132 | multiple_pass_counter, 133 | visited_counter); 134 | 135 | // By Adding the nodes of the path to the set, we only retain the unique elements 136 | std::set pathSet(path.begin(), path.end()); 137 | 138 | // Because the area is 4*4 but can only visit half the map, half the 4x4 area should be covered 139 | ASSERT_EQ((4 * 4) / 2, pathSet.size()); 140 | } 141 | 142 | /* 143 | * On a map with a wall almost blocking off a half of the map, but leaving a gap to the other side, 144 | * spiral_stc should still cover all reachable nodes 145 | */ 146 | TEST(TestSpiralStc, testFillMapWithWall) 147 | { 148 | /* 149 | * [s 0 1 0] 150 | * [0 0 1 0] 151 | * [0 0 1 0] 152 | * [0 0 0 0] 153 | */ 154 | std::vector > grid = makeTestGrid(4, 4, false); 155 | grid[0][2] = 1; 156 | grid[1][2] = 1; 157 | grid[2][2] = 1; 158 | 159 | Point_t start = {0, 0}; 160 | int multiple_pass_counter, visited_counter; 161 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 162 | start, 163 | multiple_pass_counter, 164 | visited_counter); 165 | 166 | // By Adding the nodes of the path to the set, we only retain the unique elements 167 | std::set pathSet(path.begin(), path.end()); 168 | 169 | // Because the area is 4*4 there is a 3-length wall 170 | ASSERT_EQ((4 * 4) - 3, pathSet.size()); 171 | } 172 | 173 | /* 174 | * This test case features a very short dead-end 175 | */ 176 | TEST(TestSpiralStc, testDeadEnd1) 177 | { 178 | /* 179 | * [0 0 1 0] 180 | * [0 1 0 0] 181 | * [0 s 0 0] 182 | * [0 0 0 0] 183 | */ 184 | std::vector > grid = makeTestGrid(4, 4, false); 185 | grid[0][2] = 1; 186 | grid[1][1] = 1; 187 | 188 | cv::Mat mapImg = drawMap(grid); 189 | 190 | Point_t start = {1, 2}; 191 | int multiple_pass_counter, visited_counter; 192 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 193 | start, 194 | multiple_pass_counter, 195 | visited_counter); 196 | 197 | cv::Mat pathImg = mapImg.clone(); 198 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 199 | cv::imwrite("/tmp/testDeadEnd1.png", pathViz); 200 | 201 | // By Adding the nodes of the path to the set, we only retain the unique elements 202 | std::set pathSet(path.begin(), path.end()); 203 | 204 | // Because the area is 4*4 and there are2 obstacle cells 205 | ASSERT_EQ((4 * 4) - 2, pathSet.size()); 206 | } 207 | 208 | /* 209 | * This test case is an extension of testDeadEnd1, where the top row is also covered as an obstacle. 210 | * The top row is covered but the obstacle from testDeadEnd1 is shifted downwards 211 | * in an attempt to see if SpiralSTC also fails when a dead-end is not on the edge of the map 212 | * (but below a row of obstacles) 213 | */ 214 | TEST(TestSpiralStc, testDeadEnd1WithTopRow) 215 | { 216 | /* 217 | * [1 1 1 1] 218 | * [0 0 1 0] 219 | * [0 1 0 0] 220 | * [0 s 0 0] 221 | * [0 0 0 0] 222 | */ 223 | std::vector > grid = makeTestGrid(4, 5, false); 224 | grid[0][0] = 1; 225 | grid[0][1] = 1; 226 | grid[0][2] = 1; 227 | grid[0][3] = 1; 228 | 229 | grid[1][2] = 1; 230 | grid[2][1] = 1; 231 | 232 | cv::Mat mapImg = drawMap(grid); 233 | 234 | Point_t start = {1, 3}; 235 | int multiple_pass_counter, visited_counter; 236 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 237 | start, 238 | multiple_pass_counter, 239 | visited_counter); 240 | 241 | cv::Mat pathImg = mapImg.clone(); 242 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 243 | cv::imwrite("/tmp/testDeadEnd1WithTopRow.png", pathViz); 244 | 245 | // By Adding the nodes of the path to the set, we only retain the unique elements 246 | std::set pathSet(path.begin(), path.end()); 247 | 248 | // Because the area is 4*5 and there are 6 cells blocked in total 249 | ASSERT_EQ((4 * 5) - 6, pathSet.size()); 250 | } 251 | 252 | /* 253 | * This test case features a very short dead-end 254 | */ 255 | TEST(TestSpiralStc, testDeadEnd2) 256 | { 257 | /* 258 | * [1 0 0 0] 259 | * [0 1 0 0] 260 | * [0 s 0 0] 261 | * [0 0 0 0] 262 | */ 263 | std::vector > grid = makeTestGrid(4, 4, false); 264 | grid[0][0] = 1; 265 | grid[1][1] = 1; 266 | 267 | cv::Mat mapImg = drawMap(grid); 268 | 269 | Point_t start = {3, 0}; 270 | int multiple_pass_counter, visited_counter; 271 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 272 | start, 273 | multiple_pass_counter, 274 | visited_counter); 275 | 276 | cv::Mat pathImg = mapImg.clone(); 277 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 278 | cv::imwrite("/tmp/testDeadEnd2.png", pathViz); 279 | 280 | // By Adding the nodes of the path to the set, we only retain the unique elements 281 | std::set pathSet(path.begin(), path.end()); 282 | 283 | // Because the area is 4*4 and there are2 obstacle cells 284 | ASSERT_EQ((4 * 4) - 2, pathSet.size()); 285 | } 286 | 287 | /* 288 | * This test case features a very short dead-end 289 | */ 290 | TEST(TestSpiralStc, testDeadEnd3) 291 | { 292 | /* 293 | * [0 0 0 0 0 0 1 0 0] 294 | * [1 0 1 0 1 1 0 0 0] 295 | * [0 0 0 0 1 2 0 0 0] 296 | * [0 0 0 0 0 0 0 0 0] 297 | * [0 0 0 0 0 0 0 0 0] 298 | * [0 0 0 0 0 0 0 0 0] 299 | */ 300 | std::vector > grid = makeTestGrid(9, 6, false); 301 | grid[0][6] = 1; 302 | grid[1][0] = 1; 303 | grid[1][2] = 1; 304 | grid[1][4] = 1; 305 | grid[1][5] = 1; 306 | grid[2][4] = 1; 307 | 308 | cv::Mat mapImg = drawMap(grid); 309 | 310 | Point_t start = {5, 2}; 311 | int multiple_pass_counter, visited_counter; 312 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 313 | start, 314 | multiple_pass_counter, 315 | visited_counter); 316 | 317 | cv::Mat pathImg = mapImg.clone(); 318 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 319 | cv::imwrite("/tmp/testDeadEnd3.png", pathViz); 320 | 321 | // By Adding the nodes of the path to the set, we only retain the unique elements 322 | std::set pathSet(path.begin(), path.end()); 323 | 324 | // Because the area is 4*4 and there are2 obstacle cells 325 | ASSERT_EQ((6 * 9) - 6, pathSet.size()); 326 | } 327 | 328 | /* 329 | * This test case features a very short dead-end 330 | */ 331 | TEST(TestSpiralStc, testDeadEnd3WithTopRow) 332 | { 333 | /* 334 | * [1 1 1 1 1 1 1 1 1] 335 | * [0 0 0 0 0 0 1 0 0] 336 | * [1 0 1 0 1 1 0 0 0] 337 | * [0 0 0 0 1 2 0 0 0] 338 | * [0 0 0 0 0 0 0 0 0] 339 | * [0 0 0 0 0 0 0 0 0] 340 | * [0 0 0 0 0 0 0 0 0] 341 | */ 342 | std::vector > grid = makeTestGrid(9, 7, false); 343 | grid[0][0] = 1; 344 | grid[0][1] = 1; 345 | grid[0][2] = 1; 346 | grid[0][3] = 1; 347 | grid[0][4] = 1; 348 | grid[0][5] = 1; 349 | grid[0][6] = 1; 350 | 351 | grid[1][6] = 1; 352 | grid[2][0] = 1; 353 | grid[2][2] = 1; 354 | grid[2][4] = 1; 355 | grid[2][5] = 1; 356 | grid[3][4] = 1; 357 | 358 | cv::Mat mapImg = drawMap(grid); 359 | 360 | Point_t start = {5, 3}; // NOLINT 361 | int multiple_pass_counter, visited_counter; 362 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 363 | start, 364 | multiple_pass_counter, 365 | visited_counter); 366 | 367 | cv::Mat pathImg = mapImg.clone(); 368 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 369 | cv::imwrite("/tmp/testDeadEnd3WithTopRow.png", pathViz); 370 | 371 | // By Adding the nodes of the path to the set, we only retain the unique elements 372 | std::set pathSet(path.begin(), path.end()); 373 | 374 | // Because the area is 4*4 and there are2 obstacle cells 375 | ASSERT_EQ((7 * 9) - 6 - 7, pathSet.size()); 376 | } 377 | 378 | /** 379 | * Draw a nested vector of bools into an openCV image 380 | * @param grid 381 | * @return 2D 8-bit single-channel image 382 | */ 383 | cv::Mat drawMap(std::vector > const& grid) 384 | { 385 | int y_size = static_cast(grid.size()); 386 | int x_size = static_cast(grid[0].size()); 387 | 388 | cv::Mat mapImg = cv::Mat::zeros(y_size, x_size, CV_8U); // CV_8U 8bit unsigned int 1 channel 389 | for (int k = 0; k < y_size; k++) 390 | { 391 | for (int l = 0; l < x_size; l++) 392 | { 393 | if (grid[k][l]) 394 | { 395 | cv::rectangle(mapImg, {l, k}, {l, k}, 255); // NOLINT 396 | } 397 | } 398 | } 399 | return mapImg; 400 | } 401 | 402 | /** 403 | * Draw path on a copy of the map 404 | * This is done twice: one to serve as input for calcDifference and another is returned for visualisation purposes 405 | * @param mapImg original map with just obstacles 406 | * @param pathImg Image that will feed into calcDifference 407 | * @param start Where does the path start? 408 | * @param path the actual path to be drawn 409 | * @return 2D RGB image for visualisation purposes 410 | */ 411 | cv::Mat drawPath(const cv::Mat &mapImg, 412 | const cv::Mat &pathImg, 413 | const Point_t &start, 414 | std::list &path) 415 | { 416 | cv::Mat pathViz = cv::Mat::zeros(mapImg.cols, mapImg.rows, CV_8UC3); 417 | std::vector channels; 418 | channels.push_back(mapImg.clone()); 419 | channels.push_back(mapImg.clone()); 420 | channels.push_back(mapImg.clone()); 421 | cv::merge(channels, pathViz); 422 | 423 | int step = 0; 424 | for (std::list::iterator it = path.begin(); it != path.end(); ++it) 425 | { 426 | // std::cout << "Path at (" << it->x << ", " << it->y << ")" << std::endl; 427 | cv::rectangle(pathImg, {it->x, it->y}, {it->x, it->y}, 255); // NOLINT 428 | 429 | // Color the path in lighter and lighter color towards the end 430 | step++; 431 | int value = ((step * 200) / static_cast(path.size())) + 50; 432 | cv::Scalar color(value, 128, 128); 433 | cv::rectangle(pathViz, {it->x, it->y}, {it->x, it->y}, color); // NOLINT 434 | } 435 | 436 | // Draw the start and end in green and red, resp. 437 | cv::Scalar green(0, 255, 0); 438 | cv::Scalar red(0, 0, 255); 439 | cv::rectangle(pathViz, 440 | {start.x, start.y}, 441 | {start.x, start.y}, 442 | green); 443 | cv::rectangle(pathViz, 444 | {path.back().x, path.back().y}, 445 | {path.back().x, path.back().y}, 446 | red); 447 | return pathViz; 448 | } 449 | 450 | /** 451 | * Determine whether the drawn path covers all that can be covered 452 | * @param mapImg original map with obstacles 453 | * @param pathImg map with the path drawn into it, from drawPath 454 | * @param start where does the path start? 455 | * @return 456 | */ 457 | int calcDifference(const cv::Mat &mapImg, const cv::Mat &pathImg, const Point_t& start) 458 | { 459 | cv::Mat floodfilledImg = mapImg.clone(); 460 | cv::floodFill(floodfilledImg, {start.x, start.y}, 255); // NOLINT 461 | cv::Mat difference; 462 | cv::subtract(floodfilledImg, pathImg, difference); 463 | 464 | return cv::countNonZero(difference); 465 | } 466 | 467 | /** 468 | * Find a proper starting point in the map, i.e. any place that is not an obstacle 469 | * @param grid 470 | * @return 471 | */ 472 | Point_t findStart(std::vector > const& grid) 473 | { 474 | unsigned int seed = time(NULL); 475 | int y_size = grid.size(); 476 | int x_size = grid[0].size(); 477 | 478 | Point_t start = {rand_r(&seed) % x_size, rand_r(&seed) % y_size}; // Start in some random place 479 | while (grid[start.y][start.x]) 480 | { 481 | // Try to find a better starting point that is not inside an obstacle 482 | start = {rand_r(&seed) % x_size, rand_r(&seed) % y_size}; // Start in some random place 483 | } 484 | return start; 485 | } 486 | 487 | /* 488 | * Create a NxM map, spawn X random obstacles (so that some percentage of the map is covered by obstacles) 489 | Run coverage planning on that map and check that all reachable cells are covered (by using OpenCV Floodfill) 490 | 491 | 1. Create map with obstacles 492 | 2. Convert this to an image called mapImg 493 | 3. Copy mapImg as floodfilledImg 494 | 3. Copy mapImg as pathImg 495 | 4. Perform floodfill from start position on floodfilledImg 496 | 5. On each coordinate in path, fill pixel at that coordinate in pathImg 497 | 6. pathImg and floodfilledImg should be identical 498 | */ 499 | TEST(TestSpiralStc, testRandomMap) 500 | { 501 | // Seed pseudorandom sequence to create *reproducible test 502 | unsigned int seed = 12345; 503 | int failures = 0; 504 | int success = 0; 505 | int tests = 0; 506 | for (int i = 0; i < 5; ++i) 507 | // Or use https://github.com/google/googletest/blob/master/googletest/docs/advanced.md#repeating-the-tests 508 | { 509 | tests++; 510 | int x_size = rand_r(&seed) % 100 + 1; 511 | int y_size = rand_r(&seed) % 100 + 1; 512 | std::vector > grid = makeTestGrid(x_size, y_size, false); 513 | randomFillTestGrid(grid, 20); // ...% fill of obstacles 514 | 515 | cv::Mat mapImg = drawMap(grid); 516 | Point_t start = findStart(grid); 517 | int multiple_pass_counter, visited_counter; 518 | std::list path = full_coverage_path_planner::SpiralSTC::spiral_stc(grid, 519 | start, 520 | multiple_pass_counter, 521 | visited_counter); 522 | 523 | cv::Mat pathImg = mapImg.clone(); 524 | cv::Mat pathViz = drawPath(mapImg, pathImg, start, path); 525 | int differentPixelCount = calcDifference(mapImg, pathImg, start); 526 | if (differentPixelCount) 527 | { 528 | cv::imwrite("/tmp/" + std::to_string(i) + "_path_viz.png", pathViz); 529 | failures++; 530 | } 531 | else 532 | { 533 | success++; 534 | } 535 | EXPECT_EQ(0, differentPixelCount); 536 | } 537 | 538 | ASSERT_EQ(0, failures); 539 | ASSERT_EQ(tests, success); 540 | } 541 | 542 | // Run all the tests that were declared with TEST() 543 | int main(int argc, char **argv) 544 | { 545 | testing::InitGoogleTest(&argc, argv); 546 | return RUN_ALL_TESTS(); 547 | } 548 | --------------------------------------------------------------------------------