├── .github └── workflows │ └── main.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── doc ├── bsa.png ├── fcpp_modified_boustrophedon.png ├── fcpp_robot_0_5m_plus_tool_0_2m.png ├── fcpp_robot_0_5m_plus_tool_0_5m.png ├── robot_plus_tool.ipe └── robot_plus_tool.png ├── fcpp_plugin.xml ├── include └── full_coverage_path_planner │ ├── boustrophedon_stc.h │ ├── common.h │ └── full_coverage_path_planner.h ├── maps ├── basement.png ├── basement.yaml ├── grid.png ├── grid.yaml ├── square.png └── square.yaml ├── nodes └── coverage_progress ├── package.xml ├── src ├── boustrophedon_stc.cpp ├── common.cpp ├── full_coverage_path_planner.cpp └── full_coverage_path_planner │ ├── __init__.py │ └── stroke_joins.py └── test ├── README.md ├── full_coverage_path_planner ├── fcpp.rviz ├── move_base_sim.launch ├── param │ ├── controllers.yaml │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── planners.yaml ├── test_full_coverage_path_planner.launch ├── test_full_coverage_path_planner.test ├── test_full_coverage_path_planner_plugin.launch └── test_full_coverage_path_planner_system.py ├── include └── full_coverage_path_planner │ └── util.h ├── simple_goal.yaml └── src ├── test_boustrophedon_stc.cpp ├── test_common.cpp └── util.cpp /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - {ROS_DISTRO: melodic, ROS_REPO: main, UPSTREAM_WORKSPACE: 'github:nobleo/tracking_pid#master'} 11 | - {ROS_DISTRO: noetic, ROS_REPO: main, UPSTREAM_WORKSPACE: 'github:nobleo/tracking_pid#master'} 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v1 15 | - uses: 'ros-industrial/industrial_ci@master' 16 | env: ${{matrix.env}} 17 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++ 2 | # Edit at https://www.toptal.com/developers/gitignore?templates=cmake,visualstudiocode,c++ 3 | 4 | ### C++ ### 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | 38 | ### CMake ### 39 | CMakeLists.txt.user 40 | CMakeCache.txt 41 | CMakeFiles 42 | CMakeScripts 43 | Testing 44 | Makefile 45 | cmake_install.cmake 46 | install_manifest.txt 47 | compile_commands.json 48 | CTestTestfile.cmake 49 | _deps 50 | 51 | ### CMake Patch ### 52 | # External projects 53 | *-prefix/ 54 | 55 | ### VisualStudioCode ### 56 | .vscode/* 57 | !.vscode/settings.json 58 | !.vscode/tasks.json 59 | !.vscode/launch.json 60 | !.vscode/extensions.json 61 | *.code-workspace 62 | 63 | # Local History for Visual Studio Code 64 | .history/ 65 | 66 | ### VisualStudioCode Patch ### 67 | # Ignore all local history of files 68 | .history 69 | .ionide 70 | 71 | # End of https://www.toptal.com/developers/gitignore/api/cmake,visualstudiocode,c++ 72 | .vscode/settings.json 73 | -------------------------------------------------------------------------------- /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/boustrophedon_stc.cpp 39 | ) 40 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 41 | target_link_libraries(${PROJECT_NAME} 42 | ${catkin_LIBRARIES} 43 | ) 44 | 45 | install(TARGETS 46 | ${PROJECT_NAME} 47 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | ) 50 | 51 | install(DIRECTORY include/${PROJECT_NAME} 52 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 53 | ) 54 | 55 | install(FILES fcpp_plugin.xml 56 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 57 | ) 58 | 59 | catkin_install_python( 60 | PROGRAMS 61 | nodes/coverage_progress 62 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | if (CATKIN_ENABLE_TESTING) 66 | catkin_add_gtest(test_common test/src/test_common.cpp test/src/util.cpp src/common.cpp) 67 | 68 | catkin_add_gtest(test_boustrophedon_stc test/src/test_boustrophedon_stc.cpp test/src/util.cpp src/boustrophedon_stc.cpp src/common.cpp src/${PROJECT_NAME}.cpp) 69 | add_dependencies(test_boustrophedon_stc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 70 | target_link_libraries(test_boustrophedon_stc ${catkin_LIBRARIES}) 71 | 72 | find_package(OpenCV) 73 | include_directories(${OpenCV_INCLUDE_DIRS}) 74 | target_link_libraries(test_boustrophedon_stc ${OpenCV_LIBRARIES}) 75 | 76 | add_rostest(test/${PROJECT_NAME}/test_${PROJECT_NAME}.test) 77 | 78 | endif() 79 | 80 | roslint_cpp() 81 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Boustrophedon Full Coverage Path Planner (Modified) 2 | 3 | ## Acknowledgement 4 | 5 | This package is a modification of the Full Coverage Path Planner package from Nobleo. 6 | 7 | It is modified such that a Boustrophedon Pattern is used to plan the path rather than a Spiral algorithm in the original package. 8 | 9 | Refer to the original package here: http://wiki.ros.org/full_coverage_path_planner 10 | ## Overview 11 | 12 | This package provides an implementation of a Full Coverage Path Planner (FCPP) using the Boustrophedon Pattern. see [1] and [2]. 13 | 14 | This packages acts as a global planner plugin to the Move Base package (http://wiki.ros.org/move_base). 15 | 16 | ![BSA](doc/fcpp_modified_boustrophedon.png) 17 | 18 | The user can configure robot radius and tool radius separately: 19 | 20 | ![robot_plus_tool](doc/robot_plus_tool.png) 21 | 22 | 23 | **Keywords:** coverage path planning, move base 24 | 25 | ### Authors 26 | 27 | Apache 2.0 28 | 29 | **Package modified by Ethan Kim, ethanc.kim@uwaterloo.ca, MapaRobo Inc.** 30 | 31 | **Author(s): Yury Brodskiy, Ferry Schoenmakers, Tim Clephas, Jerrel Unkel, Loy van Beek, Cesar lopez** 32 | 33 | **Maintainer: Cesar Lopez, cesar.lopez@nobleo.nl** 34 | 35 | **Affiliation: Nobleo Projects BV, Eindhoven, the Netherlands** 36 | 37 | The Modified package has been tested under [ROS] Noetic and Ubuntu 20.04. 38 | 39 | ## Installation 40 | 41 | 42 | ### Building from Source 43 | 44 | 45 | #### Dependencies 46 | 47 | - [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), 48 | - [Move Base Flex (MBF)](http://wiki.ros.org/move_base_flex) (move base flex node) used for system testing 49 | 50 | 51 | #### Building 52 | 53 | To build from source, clone the latest version from this repository into your workspace and compile the package using 54 | 55 | cd catkin_workspace/src 56 | git clone https://github.com/nobleo/full_coverage_path_planner.git 57 | cd ../ 58 | catkin_make 59 | 60 | ### Unit Tests 61 | 62 | All tests can be run using: 63 | 64 | catkin build full_coverage_path_planner --catkin-make-args run_tests 65 | 66 | #### test_common 67 | Unit test that checks the basic functions used by the repository 68 | 69 | #### test_boustrophedon_stc 70 | Unit test that checks the basis boustrophedon algorithm for full coverage. The test is performed for different situations to check that the algorithm coverage the accessible map cells. A test is also performed in randomly generated maps. 71 | 72 | #### test_full_coverage_path_planner.test 73 | 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. 74 | 75 | 76 | ## Usage 77 | 78 | Run a full navigation example using: 79 | 80 | roslaunch full_coverage_path_planner test_full_coverage_path_planner.launch 81 | 82 | Give a 2D-goal in rviz to start path planning algorithm 83 | 84 | Depends on: 85 | 86 | [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 87 | 88 | [tracking_pid](https://github.com/nobleo/tracking_pid/) Global path tracking controller 89 | 90 | 91 | ## Launch files 92 | 93 | ### test/full_coverage_path_planner/test_full_coverage_path_planner.launch 94 | 95 | Runs the full_coverage_path_planner global planner in combination with tracking PID local planner. 96 | Moreover a coverage progress tracking node is launched to monitor the coverage progress. 97 | Mobile_robot_simulator is used to integrate cmd_vel output into TF and odometry. 98 | 99 | Arguments: 100 | 101 | * **`map`**: path to a global costmap. Default: `$(find full_coverage_path_planner)/maps/basement.yaml)` 102 | * **`target_x_vel`**: target x velocity for use in interpolator. Default: `0.2` 103 | * **`target_yaw_vel`**: target yaw velocity for use in interpolator. Default: `0.2` 104 | * **`robot_radius`**: radius of the robot for use in the global planner. Default: `0.6` 105 | * **`tool_radius`**: radius of the tool for use in the global planner. Default: `0.2` 106 | 107 | 108 | Start planning and tracking by giving a 2D nav goal. 109 | 110 | 111 | ## Nodes 112 | 113 | ### coverage_progress 114 | 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' 115 | 116 | #### Subscribed Topics 117 | 118 | * **`/tf`** ([tf2_msgs/TFMessage]) 119 | ros tf dynamic transformations 120 | * **`/tf_static`** ([tf2_msgs/TFMessage]) 121 | ros tf static transformations 122 | #### Published Topics 123 | 124 | * **`/coverage_grid`** ([nav_msgs/OccupancyGrid]) 125 | occupancy grid to visualize coverage progress 126 | * **`/coverage_progress`** ([std_msgs/Float32]) 127 | monitors coverage (from 0 none to 1 full) on the given area 128 | 129 | #### Services 130 | 131 | * **`/coverage_progress/reset`** ([std_srvs/SetBool]) 132 | resets coverage_progress node. For instance when robot position needs to be manually updated 133 | 134 | 135 | #### Parameters 136 | 137 | * **`target_area/x`**: size in x of the target area to monitor 138 | * **`target_area/y`**: size in y of the target area to monitor 139 | * **`coverage_radius`**: radius of the tool to compute coverage progress 140 | 141 | 142 | ## Plugins 143 | ### full_coverage_path_planner/BoustrophedonSTC 144 | For use in move_base(\_flex) as "base_global_planner"="full_coverage_path_planner/BoustrophedonSTC". It uses global_cost_map and global_costmap/robot_radius. 145 | 146 | #### Parameters 147 | 148 | * **`robot_radius`**: robot radius, which is used by the CPP algorithm to check for collisions with static map 149 | * **`tool_radius`**: tool radius, which is used by the CPP algorithm to discretize the space and find a full coverage plan 150 | 151 | 152 | ## References 153 | 154 | [1] Choset, Howie, and Philippe Pignon. "Coverage path planning: The boustrophedon cellular decomposition." Field and service robotics. Springer, London, 1998. 155 | [2] Zelinsky, Alexander, et al. "Planning paths of complete coverage of an unstructured environment by a mobile robot." Proceedings of international conference on advanced robotics. Vol. 13. 1993. 156 | 157 | ## Acknowledgments from Original Authors 158 | 159 | 163 | 164 | 165 | rosin_logo 167 | 168 | 169 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 170 | More information: rosin-project.eu 171 | 172 | eu_flag 174 | 175 | This project has received funding from the European Union’s Horizon 2020 176 | research and innovation programme under grant agreement no. 732287. 177 | -------------------------------------------------------------------------------- /doc/bsa.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/doc/bsa.png -------------------------------------------------------------------------------- /doc/fcpp_modified_boustrophedon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/doc/fcpp_modified_boustrophedon.png -------------------------------------------------------------------------------- /doc/fcpp_robot_0_5m_plus_tool_0_2m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/doc/fcpp_robot_0_5m_plus_tool_0_2m.png -------------------------------------------------------------------------------- /doc/fcpp_robot_0_5m_plus_tool_0_5m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/doc/fcpp_robot_0_5m_plus_tool_0_5m.png -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /doc/robot_plus_tool.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/doc/robot_plus_tool.png -------------------------------------------------------------------------------- /fcpp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Plans a path that covers all accessible points in a costmap by using Boustrophedon-STC (Spanning Tree Coverage). 5 | In essence, the robot moves forward until an obstacle or visited node is met, then turns right or left (making a boustrophedon pattern) 6 | When stuck while completing a boustrophedon pattern, use A* to get out again and start a new boustrophedon, until A* can't find a path to uncovered cells 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /include/full_coverage_path_planner/boustrophedon_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 | //?????????? What's this file for again? define the classes? 24 | 25 | #ifndef FULL_COVERAGE_PATH_PLANNER_BOUSTROPHEDON_STC_H 26 | #define FULL_COVERAGE_PATH_PLANNER_BOUSTROPHEDON_STC_H 27 | 28 | #include "full_coverage_path_planner/full_coverage_path_planner.h" 29 | namespace full_coverage_path_planner 30 | { 31 | class BoustrophedonSTC : public nav_core::BaseGlobalPlanner, private full_coverage_path_planner::FullCoveragePathPlanner 32 | { 33 | public: 34 | /** 35 | * Find a path that does the boustrophedon pattern starting from init until a dead end is reached in the grid 36 | * @param grid 2D grid of bools. true == occupied/blocked/obstacle 37 | * @param init start position 38 | * @param visited all the nodes visited by the boustrophedon pattern 39 | * @return list of nodes that form the boustrophedon pattern 40 | */ 41 | static std::list boustrophedon(std::vector > const &grid, std::list &init, 42 | std::vector > &visited); 43 | 44 | // ????????? Why is init a list? 45 | /** 46 | * Perform Boustrophedon-STC (Spanning Tree Coverage) coverage path planning. 47 | * In essence, the robot moves forward until an obstacle or visited node is met, then turns right or left (making a boustrophedon pattern) 48 | * When stuck in the middle of the boustrophedon, use A* to get out again and start a new boustrophedon, until a* can't find a path to uncovered cells 49 | * @param grid 50 | * @param init 51 | * @return 52 | */ 53 | static std::list boustrophedon_stc(std::vector > const &grid, 54 | Point_t &init, 55 | int &multiple_pass_counter, 56 | int &visited_counter); 57 | 58 | private: 59 | /** 60 | * @brief Given a goal pose in the world, compute a plan 61 | * @param start The start pose 62 | * @param goal The goal pose 63 | * @param plan The plan... filled by the planner 64 | * @return True if a valid plan was found, false otherwise 65 | */ 66 | // ?????????? How does this work again? 67 | bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 68 | std::vector &plan); 69 | 70 | /** 71 | * @brief Initialization function for the FullCoveragePathPlanner object 72 | * @param name The name of this planner 73 | * @param costmap A pointer to the ROS wrapper of the costmap to use for planning 74 | */ 75 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 76 | }; 77 | 78 | } // namespace full_coverage_path_planner 79 | #endif // FULL_COVERAGE_PATH_PLANNER_BOUSTROPHEDON_STC_H 80 | -------------------------------------------------------------------------------- /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 | inline std::ostream &operator << (std::ostream &os, gridNode_t &g) 42 | { 43 | return os << "gridNode_t(" << g.cost << ", " << g.he << ", " << g.pos << ")"; 44 | } 45 | 46 | 47 | typedef struct 48 | { 49 | float x, y; 50 | } 51 | fPoint_t; 52 | 53 | inline std::ostream &operator << (std::ostream &os, fPoint_t &p) 54 | { 55 | return os << "(" << p.x << ", " << p.y << ")"; 56 | } 57 | 58 | enum 59 | { 60 | eNodeOpen = false, 61 | eNodeVisited = true 62 | }; 63 | 64 | enum 65 | { 66 | point = 0, 67 | east = 1, 68 | west = 2, 69 | north = 3, 70 | south = 4 71 | }; 72 | 73 | /** 74 | * Find the distance from poi to the closest point in goals 75 | * @param poi Starting point 76 | * @param goals Potential next points to find the closest of 77 | * @return Distance to the closest point (out of 'goals') to 'poi' 78 | */ 79 | int distanceToClosestPoint(Point_t poi, std::list const &goals); 80 | 81 | /** 82 | * Calculate the distance between two points, squared 83 | */ 84 | int distanceSquared(const Point_t &p1, const Point_t &p2); 85 | 86 | /** 87 | * Perform A* shorted path finding from init to one of the points in heuristic_goals 88 | * @param grid 2D grid of bools. true == occupied/blocked/obstacle 89 | * @param init start position 90 | * @param cost cost of traversing a free node 91 | * @param visited grid 2D grid of bools. true == visited 92 | * @param open_space Open space that A* need to find a path towards. Only used for the heuristic and directing search 93 | * @param pathNodes nodes that form the path from init to the closest point in heuristic_goals 94 | * @return whether we resign from finding a path or not. true is we resign and false if we found a path 95 | */ 96 | bool a_star_to_open_space(std::vector > const &grid, gridNode_t init, int cost, 97 | std::vector > &visited, std::list const &open_space, 98 | std::list &pathNodes); 99 | 100 | /** 101 | * Print a grid according to the internal representation 102 | * @param grid 103 | * @param visited 104 | * @param fullPath 105 | */ 106 | void printGrid(std::vector > const& grid, 107 | std::vector > const& visited, 108 | std::list const& path); 109 | 110 | /** 111 | * Print a grid according to the internal representation 112 | * @param grid 113 | * @param visited 114 | * @param fullPath 115 | * @param start 116 | * @param end 117 | */ 118 | void printGrid(std::vector > const& grid, 119 | std::vector > const& visited, 120 | std::list const& path, 121 | gridNode_t start, 122 | gridNode_t end); 123 | 124 | /** 125 | * Print a 2D array of bools to stdout 126 | */ 127 | void printGrid(std::vector > const& grid); 128 | 129 | /** 130 | * Convert 2D grid of bools to a list of Point_t 131 | * @param grid 2D grid representing a map 132 | * @param value_to_search points matching this value will be returned 133 | * @return a list of points that have the given value_to_search 134 | */ 135 | std::list map_2_goals(std::vector > const& grid, bool value_to_search); 136 | 137 | /** 138 | * Prints pathNodes in the terminal 139 | * @param pathNodes pathNodes to be printed in the terminal 140 | */ 141 | void printPathNodes(std::list pathNodes); 142 | 143 | /** 144 | * returns true only if the desired move is valid 145 | * @param x2 x coordinate of desired position 146 | * @param y2 y coordinate of desired position 147 | * @param nCols 148 | * @param nRows 149 | * @param grid internal map representation - 2D grid of bools. true == occupied/blocked/obstacle 150 | * @param visited 2D grid of bools. true == visited 151 | */ 152 | bool validMove(int x2, int y2, int nCols, int nRows, 153 | std::vector > const& grid, 154 | std::vector > const& visited); 155 | 156 | /** 157 | * Adds node in (x2, y2) into the list of pathNodes, and marks the node as visited 158 | */ 159 | void addNodeToList(int x2, int y2, std::list& pathNodes, 160 | std::vector>& visited); 161 | 162 | /** 163 | * Returns direction in which most free space is visible when given the robot's current location 164 | * @param ignoreDir ignores a single direction specified. Pass 0 (point) to consider all four directions. 165 | */ 166 | int dirWithMostSpace(int x2, int y2, int nCols, int nRows, 167 | std::vector > const& grid, 168 | std::vector > const& visited, 169 | int ignoreDir); 170 | 171 | #endif // FULL_COVERAGE_PATH_PLANNER_COMMON_H 172 | -------------------------------------------------------------------------------- /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 Boustrophedon 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 costmap_grid_ Costmap 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 parseCostmap(costmap_2d::Costmap2D* costmap_grid_, 98 | std::vector >& grid, 99 | float robotRadius, 100 | float toolRadius, 101 | geometry_msgs::PoseStamped const& realStart, 102 | Point_t& scaledStart); 103 | 104 | /** 105 | * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile 106 | * @param cpp_grid_ ROS occupancy grid representation. Cells higher that 65 are considered occupied 107 | * @param grid internal map representation 108 | * @param tileSize size (in meters) of a cell. This can be the robot's size 109 | * @param realStart Start position of the robot (in meters) 110 | * @param scaledStart Start position of the robot on the grid 111 | * @return success 112 | */ 113 | bool parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_, 114 | std::vector >& grid, 115 | float robotRadius, 116 | float toolRadius, 117 | geometry_msgs::PoseStamped const& realStart, 118 | Point_t& scaledStart); 119 | ros::Publisher plan_pub_; 120 | ros::ServiceClient cpp_grid_client_; 121 | nav_msgs::OccupancyGrid cpp_grid_; 122 | // Using costmap instead of Occupancy Grid from map server as the costmap updates periodically. 123 | costmap_2d::Costmap2DROS* costmap_ros_; 124 | costmap_2d::Costmap2D* costmap_; 125 | float robot_radius_; 126 | float tool_radius_; 127 | float plan_resolution_; 128 | float tile_size_; 129 | fPoint_t grid_origin_; 130 | bool initialized_; 131 | geometry_msgs::PoseStamped previous_goal_; 132 | 133 | struct boustrophedon_cpp_metrics_type 134 | { 135 | int visited_counter; 136 | int multiple_pass_counter; 137 | int accessible_counter; 138 | double total_area_covered; 139 | }; 140 | boustrophedon_cpp_metrics_type boustrophedon_cpp_metrics_; 141 | }; 142 | 143 | 144 | /** 145 | * Sort function for sorting Points on distance to a POI 146 | */ 147 | struct ComparatorForPointSort 148 | { 149 | explicit ComparatorForPointSort(Point_t poi) : _poi(poi) 150 | { 151 | } 152 | 153 | bool operator()(const Point_t& first, const Point_t& second) const 154 | { 155 | return distanceSquared(first, _poi) < distanceSquared(second, _poi); 156 | } 157 | 158 | private: 159 | Point_t _poi; 160 | }; 161 | } // namespace full_coverage_path_planner 162 | #endif // FULL_COVERAGE_PATH_PLANNER_FULL_COVERAGE_PATH_PLANNER_H 163 | -------------------------------------------------------------------------------- /maps/basement.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/maps/basement.png -------------------------------------------------------------------------------- /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/grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/maps/grid.png -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /maps/square.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/maps/square.png -------------------------------------------------------------------------------- /maps/square.yaml: -------------------------------------------------------------------------------- 1 | image: square.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 | -------------------------------------------------------------------------------- /nodes/coverage_progress: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import tf2_ros 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.tf_buffer = tf2_ros.Buffer() 31 | self.listener = tf2_ros.TransformListener(self.tf_buffer) 32 | 33 | self._coverage_area = None # type: Tuple[float, float] 34 | 35 | self.coverage_resolution = None # type: float # How big is a cell [m] 36 | 37 | # How well covered is a cell after it has been covered for 1 time step 38 | self.coverage_effectivity = None # type: int 39 | 40 | self.map_frame = None # type: str 41 | self.coverage_frame = None # type: str 42 | 43 | self.grid = self._initialize_map() 44 | 45 | self.progress_pub = rospy.Publisher("coverage_progress", Float32, queue_size=1) 46 | self.grid_pub = rospy.Publisher("coverage_grid", OccupancyGrid, queue_size=1) 47 | 48 | self.reset_srv = rospy.Service('reset', Trigger, self.reset) 49 | 50 | self._rate = rospy.get_param("~rate", 10.0) 51 | self._update_timer = rospy.Timer(rospy.Duration(1.0/self._rate), self._update_callback, reset=True) 52 | 53 | def _initialize_map(self): 54 | # Initialize coverage matrix 55 | 56 | # Define parameters 57 | x = rospy.get_param("~target_area/x", 10.0) # height of the area to cover, in x direction of the map 58 | y = rospy.get_param("~target_area/y", 5.0) # width of the area to cover, in y direction of the map 59 | self._coverage_area = (x, y) 60 | 61 | try: 62 | self.coverage_radius_meters = rospy.get_param("~coverage_radius") 63 | except KeyError: 64 | try: 65 | self.coverage_radius_meters = rospy.get_param("~coverage_width") / 2.0 66 | except KeyError: 67 | rospy.logerr("Specify either coverage_width or coverage_radius") 68 | raise ValueError("Neither ~coverage_radius nor ~coverage_width specified, one of these is required") 69 | 70 | self.coverage_resolution = rospy.get_param("~coverage_resolution", 0.05) # How big is a cell [m] 71 | 72 | # How much covered is a cell after it has been covered for 1 time step 73 | self.coverage_effectivity = rospy.get_param("~coverage_effectivity", 5) 74 | 75 | self.map_frame = rospy.get_param("~map_frame", "map") 76 | self.coverage_frame = rospy.get_param("~coverage_frame", "base_link") 77 | 78 | self.coverage_radius_meters += 2 * self.coverage_resolution # Compensate for discretization 79 | self.coverage_radius_cells = int((self.coverage_radius_meters) / self.coverage_resolution) 80 | 81 | grid = OccupancyGrid() 82 | grid.info.resolution = self.coverage_resolution 83 | 84 | grid.info.width = abs(int(self._coverage_area[X] / self.coverage_resolution)) 85 | grid.info.height = abs(int(self._coverage_area[Y] / self.coverage_resolution)) 86 | 87 | grid.info.origin.position.x = 0 if self._coverage_area[X] > 0 else self._coverage_area[X] 88 | grid.info.origin.position.y = 0 if self._coverage_area[Y] > 0 else self._coverage_area[Y] 89 | grid.info.origin.orientation.w = 1 90 | 91 | # Initialize OccupancyGrid to have all cells DIRTY 92 | grid.data = self.DIRTY * ones(grid.info.width * grid.info.height, dtype=int) 93 | 94 | return grid 95 | 96 | def _update_callback(self, event): 97 | # Get the position of point (0,0,0) the coverage_disk frame wrt. the map frame (which can both be remapped if need be) 98 | 99 | try: 100 | coverage_tf = self.tf_buffer.lookup_transform(self.map_frame, self.coverage_frame, rospy.Time(0)) 101 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: 102 | rospy.logwarn(e) 103 | return 104 | 105 | # Element of matrix corresponding to middle of coverage surface 106 | x_point = int((coverage_tf.transform.translation.x - self.grid.info.origin.position.x) / self.coverage_resolution) 107 | y_point = int((coverage_tf.transform.translation.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 | self.grid.header.stamp = coverage_tf.header.stamp 113 | 114 | # Loop over amount of cells covered in x (j) and y (i) direction 115 | for i in range(2 * self.coverage_radius_cells): 116 | for j in range(2 * self.coverage_radius_cells): 117 | 118 | x_index = j - self.coverage_radius_cells 119 | y_index = i - self.coverage_radius_cells 120 | 121 | array_index = x_point + x_index + self.grid.info.width * (y_point + y_index) 122 | 123 | cell_in_coverage_circle = x_index ** 2 + y_index ** 2 < self.coverage_radius_cells ** 2 124 | 125 | cell_in_grid = 0 <= x_point + x_index < abs(int(self._coverage_area[X] / self.coverage_resolution)) \ 126 | and 0 <= y_point + y_index < abs(int(self._coverage_area[Y] / self.coverage_resolution)) 127 | 128 | if cell_in_coverage_circle and cell_in_grid: 129 | self.grid.data[array_index] = max(0, self.grid.data[array_index] - self.coverage_effectivity) 130 | 131 | coverage_progress = float(sum([self.grid.data < self.DIRTY])) / (self.grid.info.width * self.grid.info.height) 132 | 133 | self.progress_pub.publish(coverage_progress) 134 | self.grid_pub.publish(self.grid) 135 | 136 | def finish_callback(self, msg): 137 | 138 | if msg: 139 | coverage_progress = float(sum([self.grid.data < self.DIRTY])) / (self.grid.info.width * self.grid.info.height) 140 | 141 | self.progress_pub.publish(coverage_progress) 142 | 143 | def reset(self, srv_request): 144 | rospy.loginfo("Reset coverage progress and grid") 145 | self.grid = self._initialize_map() 146 | return (True, "Reset coverage progress and grid") 147 | 148 | if __name__ == '__main__': 149 | rospy.init_node('coverage_progress') 150 | try: 151 | node = CoverageProgressNode() 152 | rospy.spin() 153 | except rospy.ROSInterruptException: 154 | pass 155 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | full_coverage_path_planner 4 | 0.6.5 5 | 6 | Full coverage path planning provides a move_base_flex plugin that can plan a path that will fully cover a given area. 7 | This package has been modified such that a boustophedon pattern is used rather than a spiral algorithm. 8 | 9 | Cesar Lopez 10 | Ferry Schoenmakers 11 | Tim Clephas 12 | Jerrel Unkel 13 | Loy van Beek 14 | Yury Brodskiy 15 | Ethan Kim 16 | Cesar Lopez 17 | 18 | Apache 2.0 19 | http://wiki.ros.org/full_coverage_path_planner 20 | 21 | catkin 22 | roslint 23 | rostest 24 | base_local_planner 25 | costmap_2d 26 | pluginlib 27 | nav_core 28 | roscpp 29 | tf2_ros 30 | amcl 31 | joint_state_publisher 32 | map_server 33 | move_base 34 | move_base_flex 35 | cv_bridge 36 | rosunit 37 | tracking_pid 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /src/boustrophedon_stc.cpp: -------------------------------------------------------------------------------- 1 | //STC just does boustrophon and a-start back and forth 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/boustrophedon_stc.h" 11 | #include 12 | 13 | // register this planner as a BaseGlobalPlanner plugin 14 | PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::BoustrophedonSTC, nav_core::BaseGlobalPlanner) 15 | 16 | int pattern_dir_ = point; 17 | 18 | namespace full_coverage_path_planner 19 | { 20 | void BoustrophedonSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 21 | { 22 | // ???????????? what's the name parameter? 23 | 24 | if (!initialized_) 25 | // 26 | // initialized meaning"? -> main access point to communicate with everything ros (ROS node = process)...-> node handle***-> ( change visibility )...-> 27 | { 28 | // Create a publisher to visualize the plan 29 | ros::NodeHandle private_nh("~/"); 30 | ros::NodeHandle nh, private_named_nh("~/" + name); 31 | 32 | plan_pub_ = private_named_nh.advertise("plan", 1); 33 | 34 | // from nodehandle... publisher... (Access point to ROS function)..-> create a subscriber/publisher 35 | // what's cpp-grid for?? Try to request the cpp-grid from the cpp_grid map_server-> get static map.. published by map server (png file)---> occupancy grid (map)...-> publish map 36 | //service(request directly from node) vs subscribe/publisher(calling function in other node)--> get map...-> gives the map***-> actually not using this... (not using static map but cost map) 37 | /////////////////////// why some public some private??? 38 | /////////////////////// where does the Path or Getmap come from?? 39 | /////////////////////// advertise path for?????? /what's service client for?????? 40 | 41 | cpp_grid_client_ = nh.serviceClient("static_map"); 42 | // Get the cost map: 43 | costmap_ros_ = costmap_ros; 44 | //are we making a local copy(global variable) of the costmap here?-> Yes 45 | costmap_ = costmap_ros->getCostmap(); 46 | // all with _ end are global variable... 47 | 48 | // Define robot radius (radius) parameter 49 | float robot_radius_default = 0.5f; 50 | //how does the f work? c++ 51 | private_named_nh.param("robot_radius", robot_radius_, robot_radius_default); 52 | // Define tool radius (radius) parameter (does the coverage***) robot radius-> 53 | float tool_radius_default = 0.5f; // ***** need to change to a smaller number... ( get it from param (param name to radius (navigation.launch under robot core to get it).. 54 | // 55 | private_named_nh.param("tool_radius", tool_radius_, tool_radius_default); 56 | initialized_ = true; 57 | } 58 | } 59 | //boustrophedon (only the mountain pattern up and down)... stc(spanning key tree coverage----> reach deadend... still have area to cover (a-star)...-> 60 | 61 | 62 | std::list BoustrophedonSTC::boustrophedon(std::vector > const& grid, std::list& init, 63 | std::vector >& visited) 64 | 65 | /// 66 | std::list BoustrophedonSTC::boustrophedon(std::vector > const& grid, std::list& init, 67 | std::vector >& visited, /*/GLOBAL VAR (VERT/HORIZONTAL)/*/) 68 | { 69 | int dx, dy, x2, y2, i, nRows = grid.size(), nCols = grid[0].size(); 70 | // Mountain pattern filling of the open space 71 | // Copy incoming list to 'end' 72 | std::list pathNodes(init); 73 | // Set starting pos 74 | x2 = pathNodes.back().pos.x; 75 | y2 = pathNodes.back().pos.y; 76 | // set initial direction based on space visible from initial pos 77 | /*/ IF GLOBAL VAR = 0*/ 78 | 79 | int robot_dir = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, point); 80 | /* GLOBAL VAR++*/ 81 | /*/ if global var ==1 /*/ 82 | int robot_dir = 83 | 84 | 85 | 86 | 87 | // set dx and dy based on robot_dir 88 | switch(robot_dir) { 89 | case east: // 1 90 | dx = +1; 91 | dy = 0; 92 | break; 93 | case west: // 2 94 | dx = -1; 95 | dy = 0; 96 | break; 97 | case north: // 3 98 | dx = 0; 99 | dy = +1; 100 | break; 101 | case south: // 4 102 | dx = 0; 103 | dy = -1; 104 | break; 105 | default: 106 | ROS_ERROR("Full Coverage Path Planner: NO INITIAL ROBOT DIRECTION CALCULATED. This is a logic error that must be fixed by editing boustrophedon_stc.cpp. Will travel east for now."); 107 | robot_dir = east; 108 | dx = +1; 109 | dy = 0; 110 | break; 111 | } 112 | 113 | bool done = false; 114 | while (!done) 115 | { 116 | // 1. drive straight until not a valid move (hit occupied cell or at end of map) 117 | 118 | bool hitWall = false; 119 | while(!hitWall) { 120 | x2 += dx; 121 | y2 += dy; 122 | if (!validMove(x2, y2, nCols, nRows, grid, visited)) 123 | { 124 | hitWall = true; 125 | x2 = pathNodes.back().pos.x; 126 | y2 = pathNodes.back().pos.y; 127 | break; 128 | } 129 | if (!hitWall) { 130 | addNodeToList(x2, y2, pathNodes, visited); 131 | } 132 | } 133 | 134 | // 2. check left and right after hitting wall, then change direction 135 | if (robot_dir == north || robot_dir == south) 136 | { 137 | // if going north/south, then check if (now if it goes east/west is valid move, if it's not, then deadend) 138 | if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited) 139 | && !validMove(x2 - 1, y2, nCols, nRows, grid, visited)) { 140 | // dead end, exit 141 | done = true; 142 | break; 143 | } else if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited)) { 144 | // east is occupied, travel towards west 145 | x2--; 146 | pattern_dir_ = west; 147 | } else if (!validMove(x2 - 1, y2, nCols, nRows, grid, visited)) { 148 | // west is occupied, travel towards east 149 | x2++; 150 | pattern_dir_ = east; 151 | } else { 152 | 153 | // both sides are opened. If don't have a prefered turn direction, travel towards most open direction 154 | if (!(pattern_dir_ == east || pattern_dir_ == west)) { 155 | if (validMove(x2, y2 + 1, nCols, nRows, grid, visited)) { 156 | pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, north); 157 | } else { 158 | pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, south); 159 | } 160 | ROS_INFO("rotation dir with most space successful"); 161 | } 162 | // Get into this following state-> (blocked or visited. valid move) preferred turn direction ***-> variable pattern direction***=> top if right here***-> pattern direction not East r West***-> ( if no preferred turn direction---> travel to most open) 163 | if (pattern_dir_ = east) { 164 | x2++; 165 | } else if (pattern_dir_ = west) { 166 | x2--; 167 | } 168 | } 169 | 170 | // add Node to List 171 | addNodeToList(x2, y2, pathNodes, visited); 172 | 173 | // change direction 180 deg (this is when after hit wall, increment by 1 node, then head backwards... this gets added to path list when the loop goes back up) 174 | if (robot_dir == north) { 175 | robot_dir = south; 176 | dy = -1; 177 | } else if (robot_dir == south) { 178 | robot_dir = north; 179 | dy = 1; 180 | } 181 | } 182 | else if (robot_dir == east || robot_dir == west) 183 | { 184 | if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited) 185 | && !validMove(x2, y2 - 1, nCols, nRows, grid, visited)) { 186 | // dead end, exit 187 | done = true; 188 | break; 189 | } else if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited)) { 190 | // north is occupied, travel towards south 191 | y2--; 192 | pattern_dir_ = south; 193 | } else if (!validMove(x2, y2 - 1, nCols, nRows, grid, visited)) { 194 | // south is occupied, travel towards north 195 | y2++; 196 | pattern_dir_ = north; 197 | } else { 198 | // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge 199 | if (!(pattern_dir_ == north || pattern_dir_ == south)) { 200 | if (validMove(x2 + 1, y2, nCols, nRows, grid, visited)) { 201 | pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, east); 202 | } else { 203 | pattern_dir_ = dirWithMostSpace(x2, y2, nCols, nRows, grid, visited, west); 204 | } 205 | } 206 | if (pattern_dir_ = north) { 207 | y2++; 208 | } else if (pattern_dir_ = south) { 209 | y2--; 210 | } 211 | } 212 | 213 | // add Node to List 214 | addNodeToList(x2, y2, pathNodes, visited); 215 | 216 | // change direction 180 deg 217 | if (robot_dir == east) { 218 | robot_dir = west; 219 | dx = -1; 220 | } else if (robot_dir == west) { 221 | robot_dir = east; 222 | dx = 1; 223 | } 224 | } 225 | } 226 | // Log 227 | // printPathNodes(pathNodes); 228 | return pathNodes; 229 | } 230 | 231 | 232 | std::list BoustrophedonSTC::boustrophedon_stc(std::vector > const& grid, 233 | Point_t& init, 234 | int &multiple_pass_counter, 235 | int &visited_counter) 236 | { 237 | // what's multiple pass counter-> while update visited (output statistical....-> log ) 238 | 239 | int x, y, nRows = grid.size(), nCols = grid[0].size(); 240 | pattern_dir_ = point; 241 | //initialize something no direction associated with it yet***-> (Default) 242 | // Initial node is initially set as visited so it does not count 243 | multiple_pass_counter = 0; 244 | visited_counter = 0; 245 | 246 | std::vector> visited; 247 | visited = grid; // Copy grid matrix 248 | x = init.x; 249 | y = init.y; 250 | 251 | // add initial point to pathNodes 252 | std::list pathNodes; 253 | std::list fullPath; 254 | 255 | addNodeToList(x, y, pathNodes, visited); 256 | 257 | std::list goals = map_2_goals(visited, eNodeOpen); // Retrieve all goalpoints (Cells not visited)--- all open cells 258 | /////////// 259 | std::cout << "Goals Left: " << goals.size() << std::endl; 260 | // how many goals to start with???(all cells not visited?) 261 | std::list::iterator it; 262 | 263 | #ifdef DEBUG_PLOT 264 | ROS_INFO("Grid before walking is: "); 265 | printGrid(grid, visited, fullPath); 266 | #endif 267 | 268 | while (goals.size() != 0) 269 | { 270 | // boustrophedon pattern from current position 271 | //goal ****- 272 | pathNodes = boustrophedon(grid, pathNodes, visited); 273 | // return a list of points*** 274 | #ifdef DEBUG_PLOT 275 | ROS_INFO("Visited grid updated after boustrophedon:"); 276 | printGrid(grid, visited, pathNodes, PatternStart, pathNodes.back()); 277 | #endif 278 | /////// 279 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 280 | { 281 | Point_t newPoint = { it->pos.x, it->pos.y }; 282 | //?????is this a pointer or another operation? (Above). 283 | visited_counter++; 284 | fullPath.push_back(newPoint); 285 | //what is fullpath pushback again?-> push all the points in to the path 286 | } 287 | 288 | ////////////////////////////////// where is it marking all boustrphedon is visited? 289 | 290 | goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints 291 | // ????????? why except last element? Remove all elements from pathNodes list except last element. 292 | // The last point is the starting point for a new search and A* extends the path from there on 293 | 294 | //????? what are we doing here again? why are we erasing the elements???-> is it to start the new path? 295 | 296 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 297 | visited_counter--; // First point is already counted as visited 298 | // Plan to closest open Node using A* 299 | // Pathnodes.back(is starting point)`goals` is essentially the map, so we use `goals` to determine the distance from the end of a potential path 300 | // to the nearest free space 301 | bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes); 302 | if (resign) 303 | { 304 | ROS_WARN("A_star_to_open_space is resigning! This may be due to the open cells outside of the obstacle boundary. Goals Left: %u", goals.size()); 305 | break; 306 | } 307 | 308 | // Update visited grid 309 | for (it = pathNodes.begin(); it != pathNodes.end(); ++it) 310 | { 311 | if (visited[it->pos.y][it->pos.x]) 312 | { 313 | multiple_pass_counter++; 314 | } 315 | visited[it->pos.y][it->pos.x] = eNodeVisited; 316 | } 317 | if (pathNodes.size() > 0) 318 | { 319 | multiple_pass_counter--; // First point is already counted as visited 320 | } 321 | 322 | #ifdef DEBUG_PLOT 323 | ROS_INFO("Grid with path marked as visited is:"); 324 | gridNode_t PatternStart = pathNodes.back(); 325 | printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back()); 326 | #endif 327 | 328 | } 329 | return fullPath; 330 | } 331 | 332 | bool BoustrophedonSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, 333 | std::vector& plan) 334 | { 335 | // Posestamped is the current location in cartesian from what particular frame? whats header what;s pose? 336 | if (!initialized_) 337 | { 338 | ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); 339 | return false; 340 | } 341 | else 342 | { 343 | ROS_INFO("Initialized!"); 344 | } 345 | 346 | //clear the plan, just in case 347 | plan.clear(); 348 | costmap_ = costmap_ros_->getCostmap(); 349 | //is the costmap_ros_ a global variable ? 350 | // this is updated cost map?? 351 | clock_t begin = clock(); 352 | Point_t startPoint; 353 | 354 | /********************** Get grid from server **********************/ 355 | std::vector > grid; 356 | nav_msgs::GetMap grid_req_srv; 357 | ROS_INFO("Requesting grid..."); 358 | if (!cpp_grid_client_.call(grid_req_srv)) 359 | { 360 | ROS_ERROR("Could not retrieve grid from map_server"); 361 | return false; 362 | } 363 | ROS_INFO("grid recieved!!"); 364 | 365 | ROS_INFO("Parsing grid to internal representation..."); 366 | if (!parseCostmap(costmap_, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)) 367 | { 368 | ROS_ERROR("Could not parse retrieved grid"); 369 | return false; 370 | } 371 | ROS_INFO("grid parsed!!"); 372 | 373 | #ifdef DEBUG_PLOT 374 | ROS_INFO("Start grid is:"); 375 | std::list printPath; 376 | printPath.push_back(startPoint); 377 | printGrid(grid, grid, printPath); 378 | #endif 379 | 380 | std::list goalPoints = boustrophedon_stc(grid, 381 | startPoint, 382 | boustrophedon_cpp_metrics_.multiple_pass_counter, 383 | boustrophedon_cpp_metrics_.visited_counter); 384 | 385 | //???? The above returns the path? 386 | ROS_INFO("naive cpp completed!"); 387 | ROS_INFO("Converting path to plan"); 388 | 389 | parsePointlist2Plan(start, goalPoints, plan); 390 | // Print some metrics: 391 | boustrophedon_cpp_metrics_.accessible_counter = boustrophedon_cpp_metrics_.visited_counter 392 | - boustrophedon_cpp_metrics_.multiple_pass_counter; 393 | boustrophedon_cpp_metrics_.total_area_covered = (4.0 * tool_radius_ * tool_radius_) * boustrophedon_cpp_metrics_.accessible_counter; 394 | ROS_INFO("Total visited: %d", boustrophedon_cpp_metrics_.visited_counter); 395 | ROS_INFO("Total re-visited: %d", boustrophedon_cpp_metrics_.multiple_pass_counter); 396 | ROS_INFO("Total accessible cells: %d", boustrophedon_cpp_metrics_.accessible_counter); 397 | ROS_INFO("Total accessible area: %f", boustrophedon_cpp_metrics_.total_area_covered); 398 | 399 | // TODO(CesarLopez): Check if global path should be calculated repetitively or just kept 400 | // (also controlled by planner_frequency parameter in move_base namespace) 401 | 402 | ROS_INFO("Publishing plan!"); 403 | publishPlan(plan); 404 | ROS_INFO("Plan published!"); 405 | ROS_DEBUG("Plan published"); 406 | 407 | clock_t end = clock(); 408 | double elapsed_secs = static_cast(end - begin) / CLOCKS_PER_SEC; 409 | std::cout << "elapsed time: " << elapsed_secs << "\n"; 410 | 411 | return true; 412 | } 413 | } // namespace full_coverage_path_planner 414 | -------------------------------------------------------------------------------- /src/common.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright [2020] Nobleo Technology" [legal/copyright] 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | int distanceToClosestPoint(Point_t poi, std::list const& goals) 13 | { 14 | // Return minimum distance from goals-list 15 | int min_dist = INT_MAX; 16 | //POI starting point 17 | //??????????????????????? is the list of goals, one block at a time? (distance is reading all the cells) 18 | //??????????? where does the Goal list come from and it's entirely not sorted? Also, why distance squared not regular distance? 19 | std::list::const_iterator it; 20 | for (it = goals.begin(); it != goals.end(); ++it) 21 | { 22 | int cur_dist = distanceSquared((*it), poi); 23 | if (cur_dist < min_dist) 24 | { 25 | min_dist = cur_dist; 26 | } 27 | } 28 | return min_dist; 29 | } 30 | 31 | int distanceSquared(const Point_t& p1, const Point_t& p2) 32 | { 33 | int dx = p2.x - p1.x; 34 | int dy = p2.y - p1.y; 35 | 36 | int dx2 = dx * dx; 37 | if (dx2 != 0 && dx2 / dx != dx) 38 | { 39 | throw std::range_error("Integer overflow error for the given points"); 40 | } 41 | 42 | int dy2 = dy * dy; 43 | if (dy2 != 0 && dy2 / dy != dy) 44 | { 45 | throw std::range_error("Integer overflow error for the given points"); 46 | } 47 | 48 | if (dx2 > std::numeric_limits< int >::max() - dy2) 49 | throw std::range_error("Integer overflow error for the given points"); 50 | int d2 = dx2 + dy2; 51 | 52 | return d2; 53 | } 54 | 55 | /** 56 | * Sort vector by the heuristic value of the last element 57 | * @return whether last elem. of first has a larger heuristic value than last elem of second 58 | */ 59 | // ????????????????????? why do we need to compare last element of first vs second? also, why not in .h file? 60 | bool sort_gridNodePath_heuristic_desc(const std::vector &first, const std::vector &second) 61 | { 62 | return (first.back().he > second.back().he); 63 | } 64 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 65 | //?????????????????????? don't know how this is done... 66 | bool a_star_to_open_space(std::vector > const &grid, gridNode_t init, int cost, 67 | std::vector > &visited, std::list const &open_space, 68 | std::list &pathNodes) 69 | { 70 | uint dx, dy, dx_prev, nRows = grid.size(), nCols = grid[0].size(); 71 | 72 | std::vector > closed(nRows, std::vector(nCols, eNodeOpen)); 73 | //?????????? can you help me understand this initialization... with () in middle?? matrix of values (why nrows/std::vector 74 | 75 | // All nodes in the closest list are currently still open 76 | 77 | closed[init.pos.y][init.pos.x] = eNodeVisited; // Of course we have visited the current/initial location 78 | #ifdef DEBUG_PLOT 79 | std::cout << "A*: Marked init " << init << " as eNodeVisited (true)" << std::endl; 80 | printGrid(closed); 81 | #endif 82 | 83 | std::vector > open1(1, std::vector(1, init)); // open1 is a *vector* of paths 84 | //????? how does this work???? where does this vector of path gets generated?? 85 | 86 | while (true) 87 | { 88 | #ifdef DEBUG_PLOT 89 | std::cout << "A*: open1.size() = " << open1.size() << std::endl; 90 | #endif 91 | if (open1.size() == 0) // If there are no open paths, there's no place to go and we must resign 92 | { 93 | // Empty end_node list and add init as only element 94 | pathNodes.erase(pathNodes.begin(), --(pathNodes.end())); 95 | pathNodes.push_back(init); 96 | // What does this do push init back???????????????????? do we just not go anywhere? (what does resign mean?) 97 | return true; // We resign, cannot find a path 98 | } 99 | else 100 | { 101 | // ????????????Sort elements from high to low (because sort_gridNodePath_heuristic_desc uses a > b) 102 | std::sort(open1.begin(), open1.end(), sort_gridNodePath_heuristic_desc); 103 | //???????? what's the difference between Open1.end vs open1.back? 104 | std::vector nn = open1.back(); // Get the *path* with the lowest heuristic cost 105 | open1.pop_back(); // The last element is no longer open because we use it here, so remove from open list 106 | #ifdef DEBUG_PLOT 107 | std::cout << "A*: Check out path from" << nn.front().pos << " to " << nn.back().pos 108 | << " of length " << nn.size() << std::endl; 109 | #endif 110 | 111 | // Does the path nn end in open space? 112 | if (visited[nn.back().pos.y][nn.back().pos.x] == eNodeOpen) 113 | { 114 | // If so, we found a path to open space 115 | // Copy the path nn to pathNodes so we can report that path (to get to open space) 116 | std::vector::iterator iter; 117 | for (iter = nn.begin(); iter != nn.end(); ++iter) 118 | { 119 | pathNodes.push_back((*iter)); 120 | } 121 | 122 | return false; // We do not resign, we found a path 123 | } 124 | else // Path nn does not lead to open space 125 | { 126 | if (nn.size() > 1) 127 | { 128 | // Create iterator for gridNode_t list and let it point to the last element of nn 129 | std::vector::iterator it = --(nn.end()); 130 | dx = it->pos.x - (it - 1)->pos.x; 131 | dy = it->pos.y - (it - 1)->pos.y; 132 | // TODO(CesarLopez) docs: this seems to cycle through directions 133 | // (notice the shift-by-one between both sides of the =) 134 | dx_prev = dx; 135 | dx = -dy; 136 | dy = dx_prev; 137 | } 138 | else 139 | { 140 | dx = 0; 141 | dy = 1; 142 | } 143 | 144 | // For all nodes surrounding the end of the end of the path nn 145 | for (uint i = 0; i < 4; ++i) 146 | { 147 | Point_t p2 = 148 | { 149 | static_cast(nn.back().pos.x + dx), 150 | static_cast(nn.back().pos.y + dy), 151 | }; 152 | 153 | #ifdef DEBUG_PLOT 154 | std::cout << "A*: Look around " << i << " at p2=(" << p2 << std::endl; 155 | #endif 156 | 157 | if (p2.x >= 0 && p2.x < nCols && p2.y >= 0 && p2.y < nRows) // Bounds check, do not sep out of map 158 | { 159 | // If the new node (a neighbor of the end of the path nn) is open, append it to newPath ( = nn) 160 | // and add that to the open1-list of paths. 161 | // Because of the pop_back on open1, what happens is that the path is temporarily 'checked out', 162 | // modified here, and then added back (if the condition above and below holds) 163 | if (closed[p2.y][p2.x] == eNodeOpen && grid[p2.y][p2.x] == eNodeOpen) 164 | { 165 | #ifdef DEBUG_PLOT 166 | std::cout << "A*: p2=" << p2 << " is OPEN" << std::endl; 167 | #endif 168 | std::vector newPath = nn; 169 | // # heuristic has to be designed to prefer a CCW turn 170 | Point_t new_point = { p2.x, p2.y }; 171 | gridNode_t new_node = 172 | { 173 | new_point, // Point: x,y 174 | static_cast(cost + nn.back().cost), // Cost 175 | static_cast(cost + nn.back().cost + distanceToClosestPoint(p2, open_space) + i), 176 | // Heuristic (+i so CCW turns are cheaper) 177 | }; 178 | newPath.push_back(new_node); 179 | closed[new_node.pos.y][new_node.pos.x] = eNodeVisited; // New node is now used in a path and thus visited 180 | 181 | #ifdef DEBUG_PLOT 182 | std::cout << "A*: Marked new_node " << new_node << " as eNodeVisited (true)" << std::endl; 183 | std::cout << "A*: Add path from " << newPath.front().pos << " to " << newPath.back().pos 184 | << " of length " << newPath.size() << " to open1" << std::endl; 185 | #endif 186 | open1.push_back(newPath); 187 | } 188 | #ifdef DEBUG_PLOT 189 | else 190 | { 191 | std::cout << "A*: p2=" << p2 << " is not open: " 192 | "closed[" << p2.y << "][" << p2.x << "]=" << closed[p2.y][p2.x] << ", " 193 | "grid[" << p2.y << "][" << p2.x << "]=" << grid[p2.y][p2.x] << std::endl; 194 | } 195 | #endif 196 | } 197 | #ifdef DEBUG_PLOT 198 | else 199 | { 200 | std::cout << "A*: p2=(" << p2.x << ", " << p2.y << ") is out of bounds" << std::endl; 201 | } 202 | #endif 203 | // Cycle around to next neighbor, CCW 204 | dx_prev = dx; 205 | dx = dy; 206 | dy = -dx_prev; 207 | } 208 | } 209 | } 210 | } 211 | } 212 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 213 | 214 | void printGrid(std::vector > const& grid, std::vector > const& visited, 215 | std::list const& path) 216 | { 217 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 218 | { 219 | for (uint ix = 0; ix < grid[0].size(); ++ix) 220 | { 221 | if (visited[iy][ix]) 222 | { 223 | if (ix == path.front().x && iy == path.front().y) //path.front = (starting) // whatis this kind of code???? 224 | { 225 | std::cout << "\033[1;32m▓\033[0m"; // Show starting position in green color 226 | } 227 | else if (ix == path.back().x && iy == path.back().y) path.back = red 228 | { 229 | std::cout << "\033[1;31m▓\033[0m"; // Show stopping position in red color 230 | } 231 | else if (visited[iy][ix] && grid[iy][ix]) 232 | { 233 | std::cout << "\033[1;33m▓\033[0m"; // Show walls in yellow color 234 | } 235 | else 236 | { 237 | std::cout << "\033[1;36m▓\033[0m";// what is this again? unvisited? 238 | } 239 | } 240 | else 241 | { 242 | std::cout << "\033[1;37m▓\033[0m"; 243 | } 244 | } 245 | std::cout << "\n"; 246 | } 247 | } 248 | 249 | // ????????? why do we need multiple print grid 250 | void printGrid(std::vector > const& grid, std::vector > const& visited, 251 | std::list const& path, gridNode_t start, gridNode_t end) 252 | { 253 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 254 | { 255 | for (uint ix = 0; ix < grid[0].size(); ++ix) 256 | { 257 | if (visited[iy][ix]) 258 | { 259 | if (ix == start.pos.x && iy == start.pos.y) 260 | { 261 | std::cout << "\033[1;32m▓\033[0m"; // Show starting position in green color 262 | } 263 | else if (ix == end.pos.x && iy == end.pos.y) 264 | { 265 | std::cout << "\033[1;31m▓\033[0m"; // Show stopping position in red color 266 | } 267 | else if (visited[iy][ix] && grid[iy][ix]) 268 | { 269 | std::cout << "\033[1;33m▓\033[0m"; // Show walls in yellow color 270 | } 271 | else 272 | { 273 | std::cout << "\033[1;36m▓\033[0m"; 274 | } 275 | } 276 | else 277 | { 278 | std::cout << "\033[1;37m▓\033[0m"; 279 | } 280 | } 281 | std::cout << "\n"; 282 | } 283 | } 284 | 285 | void printGrid(std::vector > const& grid) 286 | { 287 | for (uint iy = grid.size() - 1; iy >= 0; --iy) 288 | { 289 | for (uint ix = 0; ix < grid[0].size(); ++ix) 290 | { 291 | if (grid[iy][ix]) 292 | { 293 | std::cout << "\033[1;36m▓\033[0m"; 294 | } 295 | else 296 | { 297 | std::cout << "\033[1;37m▓\033[0m"; 298 | } 299 | } 300 | std::cout << "\n"; 301 | } 302 | } 303 | 304 | std::list map_2_goals(std::vector > const& grid, bool value_to_search) 305 | { 306 | std::list goals; 307 | int ix, iy; 308 | uint nRows = grid.size(); 309 | uint nCols = grid[0].size(); 310 | for (iy = 0; iy < nRows; ++(iy)) 311 | { 312 | for (ix = 0; ix < nCols; ++(ix)) 313 | { 314 | if (grid[iy][ix] == value_to_search) 315 | 316 | //?????????? is the given value to search True or False?? what is the end purpose of this function? 317 | { 318 | Point_t p = { ix, iy }; // x, y 319 | goals.push_back(p); 320 | //????????????? what's the difference on push_back? 321 | } 322 | } 323 | } 324 | return goals; 325 | } 326 | 327 | 328 | void printPathNodes(std::list pathNodes) 329 | { 330 | for (gridNode_t node : pathNodes) { 331 | std::cout << "(" << node.pos.x << ", " << node.pos.y << ")" << ": " << node.cost << " " << node.he << std::endl; 332 | } 333 | std:: cout << "--------------------------------------" << std::endl; 334 | 335 | } 336 | 337 | bool validMove(int x2, int y2, int nCols, int nRows, 338 | std::vector> const& grid, 339 | std::vector> const& visited) 340 | { 341 | return (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) // path node is within the map 342 | && (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen); // the path node is unvisited 343 | // ??????????? meaning, not visited, and no obstacles. 344 | } 345 | 346 | void addNodeToList(int x2, int y2, std::list& pathNodes, 347 | std::vector>& visited) { 348 | Point_t new_point = { x2, y2 }; 349 | gridNode_t new_node = 350 | { 351 | new_point, // Point: x,y 352 | 0, // Cost 353 | 0, // Heuristic 354 | }; 355 | pathNodes.push_back(new_node); // turn point into gridnode and pushback in to path node to add new node!! ** add make it visited 356 | visited[y2][x2] = eNodeVisited; // Close node 357 | return; 358 | } 359 | 360 | int dirWithMostSpace(int x_init, int y_init, int nCols, int nRows, 361 | std::vector > const& grid, 362 | std::vector > const& visited, 363 | int ignoreDir) { 364 | // this array stores how far the robot can travel in a straight line for each direction 365 | int free_space_in_dir[5] = {0}; 366 | // for each direction 367 | for (int i = 1; i < 5; i++) { 368 | // start from starting pos 369 | int x2 = x_init; 370 | int y2 = y_init; 371 | do { // loop until hit wall 372 | switch (i) { 373 | case east: 374 | x2++; 375 | break; 376 | case west: 377 | x2--; 378 | break; 379 | case north: 380 | y2++; 381 | break; 382 | case south: 383 | y2--; 384 | break; 385 | default: 386 | break; 387 | } 388 | free_space_in_dir[i]++; 389 | // counter for space 390 | } while (validMove(x2, y2, nCols, nRows, grid, visited)); 391 | } 392 | 393 | //????????? use the biggest value***-> 394 | // set initial direction towards direction with most travel possible 395 | 396 | int indexValue = 0; 397 | for (int i = 1; i <= 4; i++) { 398 | // std::cout << "free space in " << i << ": " << free_space_in_dir[i] << std::endl; 399 | if (free_space_in_dir[i] > indexValue && i != ignoreDir) { 400 | robot_dir = i; 401 | indexValue = free_space_in_dir[i]; 402 | } 403 | } 404 | return robot_dir; 405 | } 406 | -------------------------------------------------------------------------------- /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 | //???????????? why do you need to create gui_path AND resize the path size? (to have same size as path.size?) 52 | 53 | if (!path.empty()) 54 | { 55 | gui_path.header.frame_id = path[0].header.frame_id; 56 | gui_path.header.stamp = path[0].header.stamp; 57 | } 58 | 59 | // Extract the plan in world co-ordinates, we assume the path is all in the same frame 60 | 61 | // ????????are moving a "local" plan to the world' coordinate frame here?? how does the publishing process work? 62 | for (unsigned int i = 0; i < path.size(); i++) 63 | { 64 | gui_path.poses[i] = path[i]; 65 | } 66 | 67 | plan_pub_.publish(gui_path); 68 | //?????????? where does plan_pub_ come from? 69 | } 70 | 71 | 72 | void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamped& start, 73 | std::list const& goalpoints, 74 | std::vector& plan) 75 | { 76 | //????????? how does this work??? 77 | geometry_msgs::PoseStamped new_goal; 78 | std::list::const_iterator it, it_next, it_prev; 79 | int dx_now, dy_now, dx_next, dy_next, move_dir_now = 0, move_dir_prev = 0, move_dir_next = 0; 80 | bool do_publish = false; 81 | float orientation = eDirNone; 82 | ROS_INFO("Received goalpoints with length: %lu", goalpoints.size()); 83 | if (goalpoints.size() > 1) 84 | { 85 | for (it = goalpoints.begin(); it != goalpoints.end(); ++it) 86 | { 87 | //??????????? how does this iterator work??? 88 | it_next = it; 89 | it_next++; 90 | it_prev = it; 91 | it_prev--; 92 | 93 | // Check for the direction of movement 94 | if (it == goalpoints.begin()) 95 | { 96 | dx_now = it_next->x - it->x; 97 | dy_now = it_next->y - it->y; 98 | } 99 | else 100 | { 101 | dx_now = it->x - it_prev->x; //??????????? whats dx now???? (are we trying to get two directions???) 102 | dy_now = it->y - it_prev->y; 103 | dx_next = it_next->x - it->x; 104 | dy_next = it_next->y - it->y; 105 | } 106 | 107 | // Calculate direction enum: dx + dy*2 will give a unique number for each of the four possible directions because 108 | // of their signs: 109 | // 1 + 0*2 = 1 110 | // 0 + 1*2 = 2 111 | // -1 + 0*2 = -1 112 | // 0 + -1*2 = -2 113 | /* eDirNone = 0, 114 | eDirRight = 1, 115 | eDirUp = 2, 116 | eDirLeft = -1, 117 | eDirDown = -2,*/ 118 | move_dir_now = dx_now + dy_now * 2; 119 | move_dir_next = dx_next + dy_next * 2; 120 | 121 | // Check if this points needs to be published (i.e. a change of direction or first or last point in list) 122 | do_publish = move_dir_next != move_dir_now || it == goalpoints.begin() || 123 | (it != goalpoints.end() && it == --goalpoints.end()); 124 | move_dir_prev = move_dir_now; 125 | 126 | //????????????? publish means to get robot to turn? (do_publish) means it was successful? 127 | 128 | // Add to vector if required 129 | if (do_publish) 130 | { 131 | new_goal.header.frame_id = "map"; 132 | //?????????????? why tile size * 0.5??? 133 | new_goal.pose.position.x = (it->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; 134 | new_goal.pose.position.y = (it->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; 135 | //?????????????? how did we get the above equations?? 136 | // Calculate desired orientation to be in line with movement direction 137 | switch (move_dir_now) 138 | { 139 | case eDirNone: 140 | // Keep orientation 141 | //???????????????? how does orientation work below????????? 142 | break; 143 | case eDirRight: 144 | orientation = 0; 145 | break; 146 | case eDirUp: 147 | orientation = M_PI / 2; 148 | break; 149 | case eDirLeft: 150 | orientation = M_PI; 151 | break; 152 | case eDirDown: 153 | orientation = M_PI * 1.5; 154 | break; 155 | } 156 | new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(orientation); 157 | //???????????? is this getting info from IMU?? 158 | if (it != goalpoints.begin()) 159 | { 160 | //?????????? how do we know it has changed direction? 161 | previous_goal_.pose.orientation = new_goal.pose.orientation; 162 | //?????????????why do we republish previous goal (with current direction?) 163 | // republish previous goal but with new orientation to indicate change of direction 164 | //???????? useful when the plan is strictly followed with base_link 165 | plan.push_back(previous_goal_); 166 | } 167 | ROS_DEBUG("Voila new point: x=%f, y=%f, o=%f,%f,%f,%f", new_goal.pose.position.x, new_goal.pose.position.y, 168 | new_goal.pose.orientation.x, new_goal.pose.orientation.y, new_goal.pose.orientation.z, 169 | new_goal.pose.orientation.w); 170 | plan.push_back(new_goal); 171 | previous_goal_ = new_goal; 172 | } 173 | } 174 | } 175 | else 176 | { 177 | //??????????? when does this else statement get triggered? 178 | new_goal.header.frame_id = "map"; 179 | new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5; 180 | new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5; 181 | new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(0); 182 | plan.push_back(new_goal); 183 | } 184 | /* Add poses from current position to start of plan */ 185 | //???????????? what does it mean to add poses from current position to start of plan*** 186 | // Compute angle between current pose and first plan point 187 | //?????????? is first plan point, the first point on the path? 188 | double dy = plan.begin()->pose.position.y - start.pose.position.y; 189 | double dx = plan.begin()->pose.position.x - start.pose.position.x; 190 | // Arbitrary choice of 100.0*FLT_EPSILON to determine minimum angle precision of 1% 191 | if (!(fabs(dy) < 100.0 * FLT_EPSILON && fabs(dx) < 100.0 * FLT_EPSILON)) 192 | { 193 | //???????????? how is this used?? (when is the condition true?) 194 | // the difference between 1 and the least value greater than 1 that is representable in the given floating point type b^(1−p) 195 | // Returns the absolute value of x: |x|. 196 | 197 | 198 | //??????????? how does this work? 199 | // Add extra translation waypoint 200 | double yaw = std::atan2(dy, dx); 201 | geometry_msgs::Quaternion quat_temp = tf::createQuaternionMsgFromYaw(yaw); 202 | // ????????? why add add extra pose? 203 | geometry_msgs::PoseStamped extra_pose; 204 | extra_pose = *plan.begin(); 205 | //???????? why is this the only pointer??? 206 | extra_pose.pose.orientation = quat_temp; 207 | plan.insert(plan.begin(), extra_pose); 208 | extra_pose = start; 209 | extra_pose.pose.orientation = quat_temp; 210 | //?????????????? why extra_pose.porientation = quant temp twice???????? 211 | plan.insert(plan.begin(), extra_pose); 212 | } 213 | 214 | // Insert current pose 215 | plan.insert(plan.begin(), start); 216 | 217 | ROS_INFO("Plan ready containing %lu goals!", plan.size()); 218 | } 219 | 220 | bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_, 221 | std::vector >& grid, 222 | float robotRadius, 223 | float toolRadius, 224 | geometry_msgs::PoseStamped const& realStart, 225 | Point_t& scaledStart) 226 | { 227 | int ix, iy, nodeRow, nodeCol; 228 | uint32_t nodeSize = dmax(floor(toolRadius / costmap_grid_->getResolution()), 1); // Size of node in pixels/units 229 | //???????????? can you help me understand the diference betwene -> and also what does the "node" size mean and is get resolution 30cm? 230 | //???????????? also, what is tool radius? 231 | uint32_t nRows = costmap_grid_->getSizeInCellsY(), nCols = costmap_grid_->getSizeInCellsX(); 232 | ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize); 233 | 234 | if (nRows == 0 || nCols == 0) 235 | { 236 | return false; 237 | } 238 | 239 | // Save map origin and scaling 240 | //????????? help to diffrentiate which one is tile, which one is node and which one is getresolution????? 241 | tile_size_ = nodeSize * costmap_grid_->getResolution(); // Size of a tile in meters 242 | grid_origin_.x = costmap_grid_->getOriginX(); // x-origin in meters 243 | grid_origin_.y = costmap_grid_->getOriginY(); // y-origin in meters 244 | ROS_INFO("costmap resolution: %g", costmap_grid_->getResolution()); 245 | ROS_INFO("tile size: %g", tile_size_); 246 | ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y); 247 | //????????? is grid origin datum???? 248 | 249 | // Scale starting point 250 | scaledStart.x = static_cast(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0, 251 | floor(nCols / tile_size_))); 252 | 253 | scaledStart.y = static_cast(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0, 254 | floor(nRows / tile_size_))); 255 | 256 | 257 | ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y); 258 | ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y); 259 | 260 | // Scale grid 261 | for (iy = 0; iy < nRows; iy = iy + nodeSize) 262 | { 263 | std::vector gridRow; 264 | for (ix = 0; ix < nCols; ix = ix + nodeSize) 265 | { 266 | //?????????? where do we specify that above 65 is occupied?? 267 | bool nodeOccupied = false; 268 | for (nodeRow = 0; (nodeRow < nodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) 269 | { 270 | //???????????? what does the conditions mean?????? (nodeRow < nodeSize) && ((iy + nodeRow) < nRows) 271 | for (nodeCol = 0; (nodeCol < nodeSize) && ((ix + nodeCol) < nCols); ++nodeCol) 272 | { 273 | double mx = ix + nodeCol; 274 | double my = iy + nodeRow; 275 | if (costmap_grid_->getCost(mx, my) > costmap_2d::INSCRIBED_INFLATED_OBSTACLE) 276 | { 277 | //?????????? how does this if statement become true? 278 | nodeOccupied = true; 279 | // ROS_INFO("(%f, %f) marked occupied", mx, my); 280 | break; 281 | } 282 | } 283 | } 284 | gridRow.push_back(nodeOccupied); // what does this push_back mean?? 285 | } 286 | grid.push_back(gridRow); 287 | } 288 | return true; 289 | } 290 | 291 | // ???????????? can you help me understand why we have a parse grid and a parse costmap function??? 292 | 293 | bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_, 294 | std::vector >& grid, 295 | float robotRadius, 296 | float toolRadius, 297 | geometry_msgs::PoseStamped const& realStart, 298 | Point_t& scaledStart) 299 | { 300 | int ix, iy, nodeRow, nodeColl; 301 | uint32_t nodeSize = dmax(floor(toolRadius / cpp_grid_.info.resolution), 1); // Size of node in pixels/units 302 | uint32_t robotNodeSize = dmax(floor(robotRadius / cpp_grid_.info.resolution), 1); // RobotRadius in pixels/units 303 | //?????????????? what is robot nodesize???? 304 | uint32_t nRows = cpp_grid_.info.height, nCols = cpp_grid_.info.width; 305 | ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize); 306 | 307 | if (nRows == 0 || nCols == 0) 308 | { 309 | return false; 310 | } 311 | 312 | // Save map origin and scaling 313 | tile_size_ = nodeSize * cpp_grid_.info.resolution; // Size of a tile in meters 314 | grid_origin_.x = cpp_grid_.info.origin.position.x; // x-origin in meters 315 | grid_origin_.y = cpp_grid_.info.origin.position.y; // y-origin in meters 316 | ROS_INFO("costmap resolution: %g", cpp_grid_.info.resolution); 317 | ROS_INFO("tile size: %g", tile_size_); 318 | ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y); 319 | 320 | // Scale starting point 321 | scaledStart.x = static_cast(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0, 322 | floor(cpp_grid_.info.width / tile_size_))); 323 | scaledStart.y = static_cast(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0, 324 | floor(cpp_grid_.info.height / tile_size_))); 325 | ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y); 326 | ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y); 327 | 328 | // Scale grid 329 | for (iy = 0; iy < nRows; iy = iy + nodeSize) 330 | { 331 | std::vector gridRow; 332 | for (ix = 0; ix < nCols; ix = ix + nodeSize) 333 | { 334 | bool nodeOccupied = false; 335 | 336 | // ????????? how come scale grid has a nodeOccupied too? (snow dump area is here?) 337 | for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) 338 | { 339 | //??????????? why robotnodesize??? 340 | for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl) 341 | { 342 | int index_grid = dmax((iy + nodeRow - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)) 343 | * nCols + (ix + nodeColl - ceil(static_cast(robotNodeSize - nodeSize) / 2.0)), 0); 344 | //?????????????? how does this work???? 345 | if (cpp_grid_.data[index_grid] > 65) 346 | { 347 | //??????????????????? how does this number get calculated???? 348 | nodeOccupied = true; 349 | break; 350 | } 351 | } 352 | } 353 | gridRow.push_back(nodeOccupied); 354 | } 355 | grid.push_back(gridRow); 356 | } 357 | return true; 358 | } 359 | } // namespace full_coverage_path_planner 360 | -------------------------------------------------------------------------------- /src/full_coverage_path_planner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MapaRobo/full_coverage_path_planner/126380b993fec42e7f0366cbb29be4f380e949c2/src/full_coverage_path_planner/__init__.py -------------------------------------------------------------------------------- /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) -------------------------------------------------------------------------------- /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_boustrophedon_stc: tests static functions of boustrophedon_stc.h 9 | 10 | Besides unittests, there are also some launch files that both illustrate how to use the 11 | - BoustrophedonSTC-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 | -------------------------------------------------------------------------------- /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: 25; 255; 0 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/BoustrophedonSTC/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 -------------------------------------------------------------------------------- /test/full_coverage_path_planner/move_base_sim.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 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /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/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | transform_tolerance: 0.2 2 | 3 | map_topic: map 4 | 5 | footprint: [[0.46, 0.28], [-0.50, 0.28], [-0.50, -0.28], [0.46, -0.28]] 6 | footprint_padding: 0.05 7 | inflation_radius: 0.55 8 | 9 | #observation_sources: point_cloud_sensor 10 | #obstacle_range: 2.5 11 | #raytrace_range: 3.0 12 | #point_cloud_sensor: { sensor_frame: d435i_link, 13 | # data_type: PointCloud2, 14 | # topic: /points_filtered, 15 | # marking: true, 16 | # clearing: true, 17 | # expected_update_rate: 1.0 18 | # max_obstacle_height: 0.75 19 | # min_obstacle_height: 0.05 } -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 5.0 5 | publish_frequency: 1.0 6 | static_map: true 7 | always_send_full_costmap: true -------------------------------------------------------------------------------- /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: 5.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 -------------------------------------------------------------------------------- /test/full_coverage_path_planner/param/planners.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: 'BoustrophedonSTC' 3 | type: 'full_coverage_path_planner/BoustrophedonSTC' 4 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner.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 | 48 | 49 | 50 | 51 | 52 | 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 | 82 | 83 | 84 | 85 | 86 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.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 | -------------------------------------------------------------------------------- /test/full_coverage_path_planner/test_full_coverage_path_planner_system.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from geometry_msgs.msg import PoseStamped 4 | from nav_msgs.msg import Path 5 | from std_msgs.msg import Bool, Float32 6 | from tracking_pid.msg import traj_point, PidDebug 7 | import math 8 | import numpy as np 9 | import rospy 10 | import sys 11 | import tf 12 | import unittest 13 | 14 | PKG = 'tracking_pid' 15 | 16 | 17 | class TestTrackingPID(unittest.TestCase): 18 | 19 | def setUp(self): 20 | rospy.init_node("rostest_tracking_pid_node") 21 | self.trajectory_finished_sub = rospy.Subscriber("trajectory_finished", Bool, 22 | self.trajectory_finished_callback, queue_size=1) 23 | self.listener = tf.TransformListener() 24 | self.trajectory_finished = False 25 | 26 | def trajectory_finished_callback(self, trajectory_finished_msg): 27 | rospy.loginfo("Trajectory finished message received on topic") 28 | self.trajectory_finished = trajectory_finished_msg.data 29 | 30 | def quaternion_to_yaw(self, quaternion_in): 31 | quaternion = ( 32 | quaternion_in.x, 33 | quaternion_in.y, 34 | quaternion_in.z, 35 | quaternion_in.w) 36 | euler = tf.transformations.euler_from_quaternion(quaternion) 37 | return (euler[2] + math.pi) % (2 * math.pi) - math.pi # wrap 38 | 39 | def test_tracking_pid(self): 40 | """ Several checks are done: 41 | - Test that interpolator point and robot start moving 42 | - Test that error at all times is bounded 43 | - Test that after some time final goal is reached 44 | A path that does not start along the y-axis is expected 45 | """ 46 | p1_msg = rospy.wait_for_message("trajectory", traj_point, timeout=5) 47 | p1_yaw = self.quaternion_to_yaw(p1_msg.pose.pose.orientation) 48 | self.listener.waitForTransform('map', 'base_link', rospy.Time(0), rospy.Duration(1.0)) 49 | (trans1, rot1) = self.listener.lookupTransform('map', 'base_link', rospy.Time(0)) 50 | rospy.sleep(2.0) 51 | p2_msg = rospy.wait_for_message("trajectory", traj_point, timeout=5) 52 | p2_yaw = self.quaternion_to_yaw(p2_msg.pose.pose.orientation) 53 | 54 | if (p1_msg.pose.pose.position.x == p2_msg.pose.pose.position.x and 55 | p1_msg.pose.pose.position.y == p2_msg.pose.pose.position.y and 56 | p1_yaw == p2_yaw): 57 | self.assertTrue(False, "Trajectory point has not moved") 58 | 59 | rospy.loginfo("Wait max 140 seconds for reaching goal") 60 | test_start_time = rospy.Time.now().to_sec() 61 | while rospy.Time.now().to_sec() - test_start_time < 140.0: 62 | self.debug_msg = rospy.wait_for_message("debug", PidDebug, timeout=5) 63 | error_vec = ( 64 | self.debug_msg.error.linear.x, 65 | self.debug_msg.error.linear.y, 66 | self.debug_msg.error.linear.z) 67 | error = np.linalg.norm(error_vec) 68 | self.assertLess(error, 0.5, "Linear error greater than 0.5 m") 69 | yaw_error = self.debug_msg.error.angular.z 70 | self.assertLess(yaw_error, 0.5, "Linear error greater than 0.5 rad") 71 | self.coverage_msg = rospy.wait_for_message("coverage_progress", Float32, timeout=5) 72 | if self.trajectory_finished is True: 73 | break 74 | 75 | self.assertTrue(self.trajectory_finished, "Trajectory not finished in 140 seconds") 76 | self.assertGreaterEqual(self.coverage_msg.data, 0.95, "Coverage less than 95%") 77 | 78 | 79 | if __name__ == '__main__': 80 | import rostest 81 | rostest.rosrun(PKG, 'rostest_tracking_pid_node', TestTrackingPID) 82 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/src/test_boustrophedon_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, boustrophedon_stc should cover all the nodes of the map 39 | */ 40 | TEST(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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 | * boustrophedon_stc should still cover all reachable nodes 145 | */ 146 | TEST(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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 BoustrophedonSTC also fails when a dead-end is not on the edge of the map 212 | * (but below a row of obstacles) 213 | */ 214 | TEST(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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(TestBoustrophedonStc, 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::BoustrophedonSTC::boustrophedon_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 | -------------------------------------------------------------------------------- /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/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 | --------------------------------------------------------------------------------