├── .circleci └── config.yml ├── .github └── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md ├── .gitignore ├── CMakeLists.txt ├── Doxyfile ├── LICENSE ├── README.md ├── include ├── cgutil.hpp └── torres_etal_2016.hpp ├── launch └── cpp_uav.launch ├── package.xml ├── script └── specify_rect.py ├── src └── torres_etal_2016.cpp ├── srv └── Torres16.srv └── test ├── test_cgutil.cpp └── test_torres_etal_2016.cpp /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | jobs: 3 | build: 4 | branches: 5 | only: 6 | - master 7 | docker: 8 | - image: ubuntu:16.04 9 | steps: 10 | - checkout 11 | - run: 12 | name: apt upgrade 13 | command: apt update && apt -y upgrade 14 | - run: 15 | name: install doxygen and git 16 | command: apt install doxygen git -y 17 | - run: 18 | name: build pdf 19 | command: doxygen Doxyfile 20 | working_directory: ~/project 21 | - run: 22 | name: remove all 23 | command: ls | grep -v -E "docs" | xargs rm -rf 24 | working_directory: ~/project 25 | - run: 26 | name: move docs and remove dir 27 | command: mv docs/* . && rmdir docs 28 | working_directory: ~/project 29 | - run: 30 | name: skip fingerprint check 31 | command: | 32 | mkdir ~/.ssh 33 | ssh-keyscan github.com >> ~/.ssh/known_hosts 34 | - run: 35 | name: deploy to gh-pages branch 36 | command: | 37 | git config --global user.name "CircleCI" 38 | git config --global user.email "circleci@ghpages.com" 39 | git add . 40 | git commit -m "Build doc" 41 | git checkout -B gh-pages 42 | git push -u origin gh-pages --force 43 | working_directory: ~/project 44 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | 5 | --- 6 | 7 | **Describe the bug** 8 | A clear and concise description of what the bug is. 9 | 10 | **To Reproduce** 11 | Steps to reproduce the behavior: 12 | 1. Go to '...' 13 | 2. Click on '....' 14 | 3. Scroll down to '....' 15 | 4. See error 16 | 17 | **Expected behavior** 18 | A clear and concise description of what you expected to happen. 19 | 20 | **Screenshots** 21 | If applicable, add screenshots to help explain your problem. 22 | 23 | **Desktop (please complete the following information):** 24 | - OS: [e.g. iOS] 25 | - Browser [e.g. chrome, safari] 26 | - Version [e.g. 22] 27 | 28 | **Smartphone (please complete the following information):** 29 | - Device: [e.g. iPhone6] 30 | - OS: [e.g. iOS8.1] 31 | - Browser [e.g. stock browser, safari] 32 | - Version [e.g. 22] 33 | 34 | **Additional context** 35 | Add any other context about the problem here. 36 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | 5 | --- 6 | 7 | **Is your feature request related to a problem? Please describe.** 8 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 9 | 10 | **Describe the solution you'd like** 11 | A clear and concise description of what you want to happen. 12 | 13 | **Describe alternatives you've considered** 14 | A clear and concise description of any alternative solutions or features you've considered. 15 | 16 | **Additional context** 17 | Add any other context or screenshots about the feature request here. 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | docs -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cpp_uav) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | geometry_msgs 8 | roscpp 9 | rospy 10 | std_msgs 11 | message_generation 12 | roslint 13 | ) 14 | 15 | find_package(CGAL REQUIRED COMPONENTS Core) 16 | 17 | include( ${CGAL_USE_FILE} ) 18 | include( CGAL_CreateSingleSourceCGALProgram ) 19 | 20 | add_service_files( 21 | FILES 22 | Torres16.srv 23 | ) 24 | 25 | generate_messages( 26 | DEPENDENCIES 27 | std_msgs 28 | geometry_msgs 29 | ) 30 | 31 | catkin_package( 32 | CATKIN_DEPENDS message_runtime std_msgs geometry_msgs 33 | ) 34 | 35 | catkin_package() 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ${Boost_INCLUDE_DIRS} 41 | ) 42 | 43 | # lint 44 | roslint_cpp() 45 | roslint_python() 46 | 47 | # Executables 48 | add_executable(torres_etal_2016 src/torres_etal_2016.cpp) 49 | 50 | add_dependencies(torres_etal_2016 ${PROJECT_NAME}_generate_messages_cpp) 51 | 52 | target_link_libraries(torres_etal_2016 ${catkin_LIBRARIES}) 53 | 54 | # Tests 55 | catkin_add_gtest(test_torres_etal_2016 test/test_torres_etal_2016.cpp) 56 | catkin_add_gtest(test_cgutil test/test_cgutil.cpp) 57 | 58 | target_link_libraries(test_torres_etal_2016 ${catkin_LIBRARIES}) 59 | target_link_libraries(test_cgutil ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /Doxyfile: -------------------------------------------------------------------------------- 1 | DOXYFILE_ENCODING = UTF-8 2 | PROJECT_NAME = "UAV Coverage Path Planner" 3 | PROJECT_NUMBER = 4 | PROJECT_BRIEF = 5 | PROJECT_LOGO = 6 | OUTPUT_DIRECTORY = 7 | CREATE_SUBDIRS = NO 8 | ALLOW_UNICODE_NAMES = NO 9 | OUTPUT_LANGUAGE = English 10 | BRIEF_MEMBER_DESC = YES 11 | REPEAT_BRIEF = YES 12 | ABBREVIATE_BRIEF = "The $name class" \ 13 | "The $name widget" \ 14 | "The $name file" \ 15 | is \ 16 | provides \ 17 | specifies \ 18 | contains \ 19 | represents \ 20 | a \ 21 | an \ 22 | the 23 | ALWAYS_DETAILED_SEC = NO 24 | INLINE_INHERITED_MEMB = NO 25 | FULL_PATH_NAMES = YES 26 | STRIP_FROM_PATH = 27 | STRIP_FROM_INC_PATH = 28 | SHORT_NAMES = NO 29 | JAVADOC_AUTOBRIEF = NO 30 | QT_AUTOBRIEF = NO 31 | MULTILINE_CPP_IS_BRIEF = NO 32 | INHERIT_DOCS = YES 33 | SEPARATE_MEMBER_PAGES = NO 34 | TAB_SIZE = 4 35 | ALIASES = 36 | TCL_SUBST = 37 | OPTIMIZE_OUTPUT_FOR_C = NO 38 | OPTIMIZE_OUTPUT_JAVA = NO 39 | OPTIMIZE_FOR_FORTRAN = NO 40 | OPTIMIZE_OUTPUT_VHDL = NO 41 | EXTENSION_MAPPING = 42 | MARKDOWN_SUPPORT = YES 43 | AUTOLINK_SUPPORT = YES 44 | BUILTIN_STL_SUPPORT = NO 45 | CPP_CLI_SUPPORT = YES 46 | SIP_SUPPORT = NO 47 | IDL_PROPERTY_SUPPORT = YES 48 | DISTRIBUTE_GROUP_DOC = NO 49 | GROUP_NESTED_COMPOUNDS = NO 50 | SUBGROUPING = YES 51 | INLINE_GROUPED_CLASSES = NO 52 | INLINE_SIMPLE_STRUCTS = NO 53 | TYPEDEF_HIDES_STRUCT = NO 54 | LOOKUP_CACHE_SIZE = 0 55 | EXTRACT_ALL = YES 56 | EXTRACT_PRIVATE = NO 57 | EXTRACT_PACKAGE = NO 58 | EXTRACT_STATIC = NO 59 | EXTRACT_LOCAL_CLASSES = YES 60 | EXTRACT_LOCAL_METHODS = NO 61 | EXTRACT_ANON_NSPACES = NO 62 | HIDE_UNDOC_MEMBERS = NO 63 | HIDE_UNDOC_CLASSES = NO 64 | HIDE_FRIEND_COMPOUNDS = NO 65 | HIDE_IN_BODY_DOCS = NO 66 | INTERNAL_DOCS = NO 67 | CASE_SENSE_NAMES = NO 68 | HIDE_SCOPE_NAMES = NO 69 | HIDE_COMPOUND_REFERENCE= NO 70 | SHOW_INCLUDE_FILES = YES 71 | SHOW_GROUPED_MEMB_INC = NO 72 | FORCE_LOCAL_INCLUDES = NO 73 | INLINE_INFO = YES 74 | SORT_MEMBER_DOCS = YES 75 | SORT_BRIEF_DOCS = NO 76 | SORT_MEMBERS_CTORS_1ST = NO 77 | SORT_GROUP_NAMES = NO 78 | SORT_BY_SCOPE_NAME = NO 79 | STRICT_PROTO_MATCHING = NO 80 | GENERATE_TODOLIST = YES 81 | GENERATE_TESTLIST = YES 82 | GENERATE_BUGLIST = YES 83 | GENERATE_DEPRECATEDLIST= YES 84 | ENABLED_SECTIONS = 85 | MAX_INITIALIZER_LINES = 30 86 | SHOW_USED_FILES = YES 87 | SHOW_FILES = YES 88 | SHOW_NAMESPACES = YES 89 | FILE_VERSION_FILTER = 90 | LAYOUT_FILE = 91 | CITE_BIB_FILES = 92 | QUIET = NO 93 | WARNINGS = YES 94 | WARN_IF_UNDOCUMENTED = YES 95 | WARN_IF_DOC_ERROR = YES 96 | WARN_NO_PARAMDOC = NO 97 | WARN_AS_ERROR = NO 98 | WARN_FORMAT = "$file:$line: $text" 99 | WARN_LOGFILE = 100 | INPUT = . 101 | INPUT_ENCODING = UTF-8 102 | FILE_PATTERNS = *.c \ 103 | *.cc \ 104 | *.cxx \ 105 | *.cpp \ 106 | *.c++ \ 107 | *.java \ 108 | *.ii \ 109 | *.ixx \ 110 | *.ipp \ 111 | *.i++ \ 112 | *.inl \ 113 | *.idl \ 114 | *.ddl \ 115 | *.odl \ 116 | *.h \ 117 | *.hh \ 118 | *.hxx \ 119 | *.hpp \ 120 | *.h++ \ 121 | *.cs \ 122 | *.d \ 123 | *.php \ 124 | *.php4 \ 125 | *.php5 \ 126 | *.phtml \ 127 | *.inc \ 128 | *.m \ 129 | *.markdown \ 130 | *.md \ 131 | *.mm \ 132 | *.dox \ 133 | *.py \ 134 | *.pyw \ 135 | *.f90 \ 136 | *.f \ 137 | *.for \ 138 | *.tcl \ 139 | *.vhd \ 140 | *.vhdl \ 141 | *.ucf \ 142 | *.qsf \ 143 | *.as \ 144 | *.js \ 145 | *.launch \ 146 | *.yaml 147 | RECURSIVE = YES 148 | EXCLUDE = 149 | EXCLUDE_SYMLINKS = NO 150 | EXCLUDE_PATTERNS = 151 | EXCLUDE_SYMBOLS = 152 | EXAMPLE_PATH = 153 | EXAMPLE_PATTERNS = * 154 | EXAMPLE_RECURSIVE = NO 155 | IMAGE_PATH = 156 | INPUT_FILTER = 157 | FILTER_PATTERNS = 158 | FILTER_SOURCE_FILES = NO 159 | FILTER_SOURCE_PATTERNS = 160 | USE_MDFILE_AS_MAINPAGE = README.md 161 | SOURCE_BROWSER = YES 162 | INLINE_SOURCES = NO 163 | STRIP_CODE_COMMENTS = YES 164 | REFERENCED_BY_RELATION = NO 165 | REFERENCES_RELATION = NO 166 | REFERENCES_LINK_SOURCE = YES 167 | SOURCE_TOOLTIPS = YES 168 | USE_HTAGS = NO 169 | VERBATIM_HEADERS = YES 170 | CLANG_ASSISTED_PARSING = NO 171 | CLANG_OPTIONS = 172 | ALPHABETICAL_INDEX = YES 173 | COLS_IN_ALPHA_INDEX = 5 174 | IGNORE_PREFIX = 175 | GENERATE_HTML = YES 176 | HTML_OUTPUT = docs 177 | HTML_FILE_EXTENSION = .html 178 | HTML_HEADER = 179 | HTML_FOOTER = 180 | HTML_STYLESHEET = 181 | HTML_EXTRA_STYLESHEET = 182 | HTML_EXTRA_FILES = 183 | HTML_COLORSTYLE_HUE = 220 184 | HTML_COLORSTYLE_SAT = 100 185 | HTML_COLORSTYLE_GAMMA = 80 186 | HTML_TIMESTAMP = NO 187 | HTML_DYNAMIC_SECTIONS = NO 188 | HTML_INDEX_NUM_ENTRIES = 100 189 | GENERATE_DOCSET = NO 190 | DOCSET_FEEDNAME = "Doxygen generated docs" 191 | DOCSET_BUNDLE_ID = org.doxygen.Project 192 | DOCSET_PUBLISHER_ID = org.doxygen.Publisher 193 | DOCSET_PUBLISHER_NAME = Publisher 194 | GENERATE_HTMLHELP = NO 195 | CHM_FILE = 196 | HHC_LOCATION = 197 | GENERATE_CHI = NO 198 | CHM_INDEX_ENCODING = 199 | BINARY_TOC = NO 200 | TOC_EXPAND = NO 201 | GENERATE_QHP = NO 202 | QCH_FILE = 203 | QHP_NAMESPACE = org.doxygen.Project 204 | QHP_VIRTUAL_FOLDER = doc 205 | QHP_CUST_FILTER_NAME = 206 | QHP_CUST_FILTER_ATTRS = 207 | QHP_SECT_FILTER_ATTRS = 208 | QHG_LOCATION = 209 | GENERATE_ECLIPSEHELP = NO 210 | ECLIPSE_DOC_ID = org.doxygen.Project 211 | DISABLE_INDEX = NO 212 | GENERATE_TREEVIEW = NO 213 | ENUM_VALUES_PER_LINE = 4 214 | TREEVIEW_WIDTH = 250 215 | EXT_LINKS_IN_WINDOW = NO 216 | FORMULA_FONTSIZE = 10 217 | FORMULA_TRANSPARENT = YES 218 | USE_MATHJAX = NO 219 | MATHJAX_FORMAT = HTML-CSS 220 | MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest 221 | MATHJAX_EXTENSIONS = 222 | MATHJAX_CODEFILE = 223 | SEARCHENGINE = YES 224 | SERVER_BASED_SEARCH = NO 225 | EXTERNAL_SEARCH = NO 226 | SEARCHENGINE_URL = 227 | SEARCHDATA_FILE = searchdata.xml 228 | EXTERNAL_SEARCH_ID = 229 | EXTRA_SEARCH_MAPPINGS = 230 | GENERATE_LATEX = NO 231 | LATEX_OUTPUT = latex 232 | LATEX_CMD_NAME = latex 233 | MAKEINDEX_CMD_NAME = makeindex 234 | COMPACT_LATEX = NO 235 | PAPER_TYPE = a4 236 | EXTRA_PACKAGES = 237 | LATEX_HEADER = 238 | LATEX_FOOTER = 239 | LATEX_EXTRA_STYLESHEET = 240 | LATEX_EXTRA_FILES = 241 | PDF_HYPERLINKS = YES 242 | USE_PDFLATEX = YES 243 | LATEX_BATCHMODE = NO 244 | LATEX_HIDE_INDICES = NO 245 | LATEX_SOURCE_CODE = NO 246 | LATEX_BIB_STYLE = plain 247 | LATEX_TIMESTAMP = NO 248 | GENERATE_RTF = NO 249 | RTF_OUTPUT = rtf 250 | COMPACT_RTF = NO 251 | RTF_HYPERLINKS = NO 252 | RTF_STYLESHEET_FILE = 253 | RTF_EXTENSIONS_FILE = 254 | RTF_SOURCE_CODE = NO 255 | GENERATE_MAN = NO 256 | MAN_OUTPUT = man 257 | MAN_EXTENSION = .3 258 | MAN_SUBDIR = 259 | MAN_LINKS = NO 260 | GENERATE_XML = NO 261 | XML_OUTPUT = xml 262 | XML_PROGRAMLISTING = YES 263 | GENERATE_DOCBOOK = NO 264 | DOCBOOK_OUTPUT = docbook 265 | DOCBOOK_PROGRAMLISTING = NO 266 | GENERATE_AUTOGEN_DEF = NO 267 | GENERATE_PERLMOD = NO 268 | PERLMOD_LATEX = NO 269 | PERLMOD_PRETTY = YES 270 | PERLMOD_MAKEVAR_PREFIX = 271 | ENABLE_PREPROCESSING = YES 272 | MACRO_EXPANSION = NO 273 | EXPAND_ONLY_PREDEF = NO 274 | SEARCH_INCLUDES = YES 275 | INCLUDE_PATH = 276 | INCLUDE_FILE_PATTERNS = 277 | PREDEFINED = 278 | EXPAND_AS_DEFINED = 279 | SKIP_FUNCTION_MACROS = YES 280 | TAGFILES = 281 | GENERATE_TAGFILE = 282 | ALLEXTERNALS = NO 283 | EXTERNAL_GROUPS = YES 284 | EXTERNAL_PAGES = YES 285 | PERL_PATH = /usr/bin/perl 286 | CLASS_DIAGRAMS = YES 287 | MSCGEN_PATH = 288 | DIA_PATH = 289 | HIDE_UNDOC_RELATIONS = YES 290 | HAVE_DOT = YES 291 | DOT_NUM_THREADS = 0 292 | DOT_FONTNAME = Helvetica 293 | DOT_FONTSIZE = 10 294 | DOT_FONTPATH = 295 | CLASS_GRAPH = YES 296 | COLLABORATION_GRAPH = YES 297 | GROUP_GRAPHS = YES 298 | UML_LOOK = NO 299 | UML_LIMIT_NUM_FIELDS = 10 300 | TEMPLATE_RELATIONS = NO 301 | INCLUDE_GRAPH = YES 302 | INCLUDED_BY_GRAPH = YES 303 | CALL_GRAPH = NO 304 | CALLER_GRAPH = NO 305 | GRAPHICAL_HIERARCHY = YES 306 | DIRECTORY_GRAPH = YES 307 | DOT_IMAGE_FORMAT = png 308 | INTERACTIVE_SVG = NO 309 | DOT_PATH = 310 | DOTFILE_DIRS = 311 | MSCFILE_DIRS = 312 | DIAFILE_DIRS = 313 | PLANTUML_JAR_PATH = 314 | PLANTUML_INCLUDE_PATH = 315 | DOT_GRAPH_MAX_NODES = 50 316 | MAX_DOT_GRAPH_DEPTH = 0 317 | DOT_TRANSPARENT = NO 318 | DOT_MULTI_TARGETS = NO 319 | GENERATE_LEGEND = YES 320 | DOT_CLEANUP = YES 321 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Takaki Ueno 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # cpp_uav 2 | Coverage path planning package for UAVs 3 | 4 | ## Nodes 5 | - specify_rect.py 6 | Shows GUI to specify region for coverage path planner. 7 | 8 | - torres_etal_2016 9 | Coverage path planner based on [1] 10 | 11 | ## Launch Files 12 | - cpp_uav.launch 13 | Launches specify_rect.py and torres_etal_2016. 14 | 15 | ## Document 16 | You can build documents by running 17 | ``` 18 | doxygen Doxyfile 19 | ``` 20 | at the root of `cpp_uav` repository. 21 | And the documents will be build in `docs` directory. 22 | 23 | The document is also available on https://uenota.github.io/cpp_uav/. 24 | 25 | ## Reference 26 | [1] Marina Torres, David A. Pelta, José L. Verdegay, Juan C. Torres, 27 | Coverage path planning with unmanned aerial vehicles for 3D terrain reconstruction, 28 | In Expert Systems with Applications, 29 | Volume 55, 2016, Pages 441-451, ISSN 0957-4174, (https://doi.org/10.1016/j.eswa.2016.02.007). 30 | -------------------------------------------------------------------------------- /include/cgutil.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file torres_etal_2016.hpp 3 | * @brief Utility for computational geometry 4 | * @author Takaki Ueno 5 | */ 6 | 7 | /* 8 | * Copyright (c) 2017 Takaki Ueno 9 | * Released under the MIT license 10 | */ 11 | 12 | #ifndef INCLUDED_cgutil_hpp_ 13 | #define INCLUDED_cgutil_hpp_ 14 | 15 | // c++ libraries 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | // CGAL 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // roscpp 32 | #include 33 | 34 | // geometry_msgs 35 | #include 36 | 37 | using PointVector = std::vector; 38 | using LineSegment = std::array; 39 | using LineSegmentVector = std::vector; 40 | 41 | // type aliases for cgal 42 | using K = CGAL::Exact_predicates_inexact_constructions_kernel; 43 | using Traits = CGAL::Partition_traits_2; 44 | using Polygon_2 = Traits::Polygon_2; 45 | using Point_2 = Traits::Point_2; 46 | using Vertex_iterator = Polygon_2::Vertex_const_iterator; 47 | using Polygon_list = std::list; 48 | 49 | /** 50 | * @brief Calculates signed area of given triangle 51 | * @param p1 The origin of vector \f$ \vec{p_1p_2} \f$ and \f$ \vec{p_1p_3} \f$ 52 | * @param p2 The end point of vector \f$ \vec{p_1p_2} \f$ 53 | * @param p3 The end point of vector \f$ \vec{p_1p_3} \f$ 54 | * @return Signed area of given triangle 55 | * 56 | * @details 57 | * Signed area of triangle \f$ S(p1, p2, p3) \f$ is 58 | * half of the outer product of vector \f$ \vec{p_1p_2} \f$ and \f$ \vec{p_1p_3} 59 | * \f$.\n 60 | * \f[ S(p_1, p_2, p_3) = \frac{1}{2} \vec{p_1p_2} \times \vec{p_1p_3}\f] \n 61 | * And that can be written as follows,\n 62 | * \f{eqnarray*}{ 63 | * S(p_1, p_2, p_3) & = & \frac{1}{2} \left| 64 | * \begin{array}{ccc} 65 | * p_1.x & p_1.y & 1 \\ 66 | * p_2.x & p_2.y & 1 \\ 67 | * p_3.x & p_3.y & 1 68 | * \end{array} 69 | * \right| \\ 70 | * & = & p_1.x(p_2.y - p_3.y) - p_1.y(p_2.x - p_3.x) - (p_2.x\times 71 | * p_3.y - p_2.y\times p_3.x) 72 | * \f} 73 | */ 74 | inline double calculateSignedArea(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2, 75 | const geometry_msgs::Point& p3) 76 | { 77 | return p1.x * (p2.y - p3.y) - p1.y * (p2.x - p3.x) + (p2.x * p3.y - p2.y * p3.x); 78 | } 79 | 80 | /** 81 | * @brief Check equality of two points 82 | * @param p1 83 | * @param p2 84 | * @return bool 85 | * @details See https://stackoverflow.com/questions/4010240/comparing-doubles 86 | */ 87 | bool operator==(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) 88 | { 89 | bool x = 90 | p1.x == p2.x || std::abs(p1.x - p2.x) < std::abs(std::min(p1.x, p2.x)) * std::numeric_limits::epsilon(); 91 | bool y = 92 | p1.y == p2.y || std::abs(p1.y - p2.y) < std::abs(std::min(p1.y, p2.y)) * std::numeric_limits::epsilon(); 93 | bool z = 94 | p1.z == p2.z || std::abs(p1.z - p2.z) < std::abs(std::min(p1.z, p2.z)) * std::numeric_limits::epsilon(); 95 | 96 | return x and y and z; 97 | } 98 | 99 | /** 100 | * @brief Check equality of two points 101 | * @param p1 102 | * @param p2 103 | * @return bool 104 | */ 105 | bool operator!=(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) 106 | { 107 | return !(p1 == p2); 108 | } 109 | 110 | /** 111 | * @brief Generate Vector of line segment from given PointVector 112 | * @param vec 113 | * @param isClosed Set true if the first point and the last are connected 114 | * @return LineSegmentVector Vector of line segment a.k.a. std::vector> 115 | */ 116 | LineSegmentVector generateEdgeVector(const PointVector& vec, bool isClosed) 117 | { 118 | LineSegmentVector edgeVector; 119 | 120 | // break if vector is empty 121 | if (vec.empty() == true) 122 | { 123 | return edgeVector; 124 | } 125 | 126 | for (int i = 0; i < vec.size(); ++i) 127 | { 128 | LineSegment edge; 129 | 130 | edge.at(0) = vec.at(i); 131 | 132 | if (i < vec.size() - 1) 133 | { 134 | // except for the last vertex 135 | edge.at(1) = vec.at(i + 1); 136 | } 137 | else 138 | { 139 | // for the last vertex 140 | edge.at(1) = vec.at(0); 141 | if (not isClosed) 142 | { 143 | break; 144 | } 145 | } 146 | 147 | edgeVector.push_back(edge); 148 | } 149 | return edgeVector; 150 | } 151 | 152 | /** 153 | * @brief Calculates distance between given two points 154 | * @param p1 155 | * @param p2 156 | * @return double Distance between two points 157 | */ 158 | inline double calculateDistance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) 159 | { 160 | return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2)); 161 | } 162 | 163 | /** 164 | * @brief Calculates angle between segment p1p2 and p1p3 165 | * @param p1 A vertex which is the origin of segment p1p2 and p1p3 166 | * @param p2 The other point of segment p1p2 167 | * @param p3 The other point of segment p1p3 168 | * @return Angle between segment p1p2 and p1p3 in radian [0, pi) 169 | */ 170 | double calculateVertexAngle(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2, 171 | const geometry_msgs::Point& p3) 172 | { 173 | // Length of edges composed of vertices 174 | // e1: (p1, p2) 175 | // e2: (p2, p3) 176 | // e3: (p3, p1) 177 | double lenE1, lenE2, lenE3; 178 | lenE1 = calculateDistance(p2, p1); 179 | lenE2 = calculateDistance(p2, p3); 180 | lenE3 = calculateDistance(p3, p1); 181 | 182 | // Cosine of angle between segment p1p2 and p1p3 (Law of cosines) 183 | double cosineP1; 184 | cosineP1 = (std::pow(lenE1, 2) + std::pow(lenE3, 2) - std::pow(lenE2, 2)) / (2 * lenE1 * lenE3); 185 | 186 | // vertex angle is 0.0 if lenE1 or lenE3 is zero 187 | // that means p1 and p2 or p1 and p3 is the same point 188 | if (std::isnan(cosineP1) != 0) 189 | { 190 | return 0.0; 191 | } 192 | 193 | return std::acos(cosineP1); 194 | } 195 | 196 | /** 197 | * @brief Calculates angle between segment p1p2 and horizontal line 198 | * @param p1 A vertex which is the origin of segment p1p2 199 | * @param p2 The other vertex of segment p1p2 200 | * @return Vertex angle of p1 in radian [-pi, pi] 201 | */ 202 | double calculateHorizontalAngle(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) 203 | { 204 | geometry_msgs::Point p3; 205 | p3.x = p1.x + 1.0; 206 | p3.y = p1.y; 207 | return p1.y >= p2.y ? -calculateVertexAngle(p1, p2, p3) : calculateVertexAngle(p1, p2, p3); 208 | } 209 | 210 | /** 211 | * @brief Calculates distance between given edge and vertex 212 | * @param edge An edge of given polygon 213 | * @param vertex A vertex of given polygon 214 | * @return double Distance between edge and vertex 215 | */ 216 | double calculateDistance(const LineSegment& edge, const geometry_msgs::Point& vertex) 217 | { 218 | // Vertices of triangle 219 | geometry_msgs::Point pointA, pointB; 220 | pointA = edge.front(); 221 | pointB = edge.back(); 222 | 223 | // Calculate length of each edge 224 | // Edge A: An edge facing vertex A 225 | // Edge B: An edge facing vertex B 226 | double lenEdgeA, lenEdgeB; 227 | lenEdgeA = calculateDistance(pointB, vertex); 228 | lenEdgeB = calculateDistance(vertex, pointA); 229 | 230 | // Vertex angles 231 | // alpha: vertex angle of pointA 232 | // beta: vertex angle of pointB 233 | double alpha, beta; 234 | alpha = calculateVertexAngle(pointA, pointB, vertex); 235 | beta = calculateVertexAngle(pointB, pointA, vertex); 236 | 237 | double distance = alpha < M_PI_2 ? std::sin(alpha) * lenEdgeB : std::sin(beta) * lenEdgeA; 238 | 239 | return distance; 240 | } 241 | 242 | /** 243 | * @brief Returns convex hull of given points 244 | * @param points A set of points in the plane 245 | * @return Convex hull of given points 246 | * 247 | * This function is based on graham scan algorithm 248 | */ 249 | PointVector computeConvexHull(PointVector points) 250 | { 251 | PointVector convexHull; 252 | 253 | if (points.empty() or points.size() < 3) 254 | { 255 | return convexHull; 256 | } 257 | 258 | // remove points that have same coodinate with other points 259 | for (size_t i = 0; i < points.size() - 1; ++i) 260 | { 261 | if (points.at(i).x == points.at(i + 1).x and points.at(i).y == points.at(i + 1).y) 262 | { 263 | points.erase(points.begin() + i); 264 | } 265 | } 266 | 267 | if (points.size() < 3) 268 | { 269 | return convexHull; 270 | } 271 | 272 | // sort by vertex's y coordinate in an ascending order 273 | std::stable_sort(points.begin(), points.end(), 274 | [](const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { return p1.y < p2.y; }); 275 | 276 | // point with minimum y coordinate 277 | geometry_msgs::Point pointMinY = points.front(); 278 | points.erase(points.begin()); 279 | 280 | // sort by an angle between a segment composed of pointMinY and pj (in a set 281 | // of points) and horizontal line 282 | // in an ascending order 283 | std::stable_sort(points.begin(), points.end(), [&](const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { 284 | return calculateHorizontalAngle(pointMinY, p1) < calculateHorizontalAngle(pointMinY, p2); 285 | }); 286 | 287 | // add pointMinY in convex hull 288 | convexHull.push_back(pointMinY); 289 | 290 | // add the point with minimum angle 291 | convexHull.push_back(points.front()); 292 | points.erase(points.begin()); 293 | 294 | for (const auto& point : points) 295 | { 296 | for (std::size_t i = convexHull.size() - 1; i > 1; --i) 297 | { 298 | if (calculateSignedArea(convexHull.at(i - 1), convexHull.at(i), point) >= 0) 299 | { 300 | break; 301 | } 302 | 303 | convexHull.pop_back(); 304 | } 305 | convexHull.push_back(point); 306 | } 307 | 308 | geometry_msgs::Point origin; 309 | origin.x = 0; 310 | origin.y = 0; 311 | std::stable_sort(convexHull.begin(), convexHull.end(), 312 | [&](const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { 313 | return calculateHorizontalAngle(origin, p1) < calculateHorizontalAngle(origin, p2); 314 | }); 315 | 316 | return convexHull; 317 | } 318 | 319 | /** 320 | * @brief Checks if given polygon is convex or not 321 | * @param points Points consisting of polygon is to be checked 322 | * @return True if given polygon is convex, false if it's not convex 323 | */ 324 | inline bool isConvex(PointVector points) 325 | { 326 | return computeConvexHull(points).size() == points.size(); 327 | } 328 | 329 | /** 330 | * @brief Checks if given edges intersect each other 331 | * @param edge1 An edge 332 | * @param edge2 An edge 333 | * @return True if two edges intersect 334 | */ 335 | bool hasIntersection(const LineSegment& edge1, const LineSegment& edge2) 336 | { 337 | geometry_msgs::Point p1, p2, p3, p4; 338 | 339 | try 340 | { 341 | p1 = edge1.at(0); 342 | p2 = edge1.at(1); 343 | p3 = edge2.at(0); 344 | p4 = edge2.at(1); 345 | } 346 | catch (std::out_of_range& ex) 347 | { 348 | ROS_ERROR("%s", ex.what()); 349 | return false; 350 | } 351 | 352 | bool condA = ((p1.x - p2.x) * (p3.y - p1.y) + (p1.y - p2.y) * (p1.x - p3.x)) * 353 | ((p1.x - p2.x) * (p4.y - p1.y) + (p1.y - p2.y) * (p1.x - p4.x)) < 354 | 0; 355 | bool condB = ((p3.x - p4.x) * (p1.y - p3.y) + (p3.y - p4.y) * (p3.x - p1.x)) * 356 | ((p3.x - p4.x) * (p2.y - p3.y) + (p3.y - p4.y) * (p3.x - p2.x)) < 357 | 0; 358 | 359 | if (condA and condB) 360 | { 361 | return true; 362 | } 363 | else 364 | { 365 | return false; 366 | } 367 | } 368 | 369 | /** 370 | * @brief Checks if given vectors of edges have at least one intersection 371 | * @param vec1 Vector of line segments 372 | * @param vec2 Vector of line segments 373 | * @return True if given two vectors of edges have at least one intersection 374 | */ 375 | bool hasIntersection(const LineSegmentVector& vec1, const LineSegmentVector& vec2) 376 | { 377 | for (const auto& segment1 : vec1) 378 | { 379 | for (const auto& segment2 : vec2) 380 | { 381 | if (hasIntersection(segment1, segment2) == true) 382 | { 383 | return true; 384 | } 385 | } 386 | } 387 | 388 | return false; 389 | } 390 | 391 | /** 392 | * @brief Find the location where given edges intersect each other 393 | * @param edge1 394 | * @param edge2 395 | * @return geometry_msgs::Point Point of intersection 396 | * @details See http://mf-atelier.sakura.ne.jp/mf-atelier/modules/tips/program/algorithm/a1.html 397 | */ 398 | geometry_msgs::Point localizeIntersection(const LineSegment& edge1, const LineSegment& edge2) 399 | { 400 | geometry_msgs::Point p1, p2, p3, p4; 401 | 402 | try 403 | { 404 | p1 = edge1.at(0); 405 | p2 = edge1.at(1); 406 | p3 = edge2.at(0); 407 | p4 = edge2.at(1); 408 | } 409 | catch (const std::out_of_range& ex) 410 | { 411 | ROS_ERROR("%s", ex.what()); 412 | } 413 | 414 | double xi, eta, delta; 415 | xi = (p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y); 416 | eta = -(p2.y - p1.y) * (p4.x - p1.x) + (p2.x - p1.x) * (p4.y - p1.y); 417 | delta = (p4.y - p3.y) * (p2.x - p1.x) - (p4.x - p3.x) * (p2.y - p1.y); 418 | 419 | double lambda, mu; 420 | lambda = xi / delta; 421 | mu = eta / delta; 422 | 423 | geometry_msgs::Point intersection; 424 | 425 | intersection.x = p1.x + lambda * (p2.x - p1.x); 426 | intersection.y = p1.y + lambda * (p2.y - p1.y); 427 | 428 | return intersection; 429 | } 430 | 431 | /** 432 | * @brief Rotate points by given angle around the origin 433 | * @param points Points to be rotated 434 | * @param angle_rad Rotation angle in radian 435 | * @return PointVector Rotated points 436 | */ 437 | PointVector rotatePoints(const PointVector& points, double angle_rad) 438 | { 439 | std::array rotationMatrix; 440 | rotationMatrix.at(0) = std::cos(angle_rad); 441 | rotationMatrix.at(1) = -std::sin(angle_rad); 442 | rotationMatrix.at(2) = std::sin(angle_rad); 443 | rotationMatrix.at(3) = std::cos(angle_rad); 444 | 445 | PointVector rotatedPoints; 446 | 447 | for (const auto& point : points) 448 | { 449 | geometry_msgs::Point pt; 450 | pt.x = rotationMatrix.at(0) * point.x + rotationMatrix.at(1) * point.y; 451 | pt.y = rotationMatrix.at(2) * point.x + rotationMatrix.at(3) * point.y; 452 | rotatedPoints.push_back(pt); 453 | } 454 | return rotatedPoints; 455 | } 456 | 457 | /** 458 | * @brief Decompose given polygon 459 | * @param polygon Polygon to be decomposed 460 | * @return std::vector Decomposed polygons 461 | * @details 462 | * This function uses CGAL::optimal_convex_partition_2 in order to perform optimal polygon decomposition. 463 | * Note that this function has O(n^4) time complexity and O(n^3) space complexity. 464 | * Use approx_convex_partition_2 instead if the number of vertices are big because its time complexity is O(n). 465 | * But apptox_convex_partition_2 generates more polygons. 466 | * For detail, see https://doc.cgal.org/latest/Partition_2/index.html 467 | */ 468 | std::vector decomposePolygon(const PointVector& polygon) 469 | { 470 | std::vector decomposedPolygons; 471 | 472 | // generate Polygon of CGAL from PointVector 473 | Polygon_2 cgalPolygon; 474 | for (const auto& vertex : polygon) 475 | { 476 | cgalPolygon.push_back(Point_2(vertex.x, vertex.y)); 477 | } 478 | 479 | Polygon_list partialCGALPolygons; 480 | Traits partitionTraits; 481 | 482 | // note that this function has O(n^4) time complexity and O(n^3) space complexity 483 | // use approx_convex_partition_2 instead if the number of vertices are big because its time complexity is O(n) 484 | // but apptox_convex_partition_2 generates more polygons 485 | CGAL::optimal_convex_partition_2(cgalPolygon.vertices_begin(), cgalPolygon.vertices_end(), 486 | std::back_inserter(partialCGALPolygons), partitionTraits); 487 | 488 | // generate std::vector from polygon of CGAL 489 | for (const auto& partialCGALPolygon : partialCGALPolygons) 490 | { 491 | PointVector partialPolygon; 492 | for (auto itr = partialCGALPolygon.vertices_begin(); itr != partialCGALPolygon.vertices_end(); ++itr) 493 | { 494 | geometry_msgs::Point pt; 495 | pt.x = itr->x(); 496 | pt.y = itr->y(); 497 | partialPolygon.push_back(pt); 498 | } 499 | decomposedPolygons.push_back(partialPolygon); 500 | } 501 | 502 | return decomposedPolygons; 503 | } 504 | 505 | #endif -------------------------------------------------------------------------------- /include/torres_etal_2016.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file torres_etal_2016.hpp 3 | * @brief Header file for torres_etal_2016.cpp 4 | * @author Takaki Ueno 5 | */ 6 | 7 | /* 8 | * Copyright (c) 2017 Takaki Ueno 9 | * Released under the MIT license 10 | */ 11 | 12 | #ifndef INCLUDED_torres_etal_2016_hpp_ 13 | #define INCLUDED_torres_etal_2016_hpp_ 14 | 15 | // cgutil 16 | #include 17 | 18 | // cpp standard libraries 19 | #include 20 | #include 21 | #include 22 | 23 | // Boost 24 | #include 25 | #include 26 | 27 | // std_msgs 28 | #include 29 | 30 | // geometry_msgs 31 | #include 32 | #include 33 | 34 | // Service 35 | #include "cpp_uav/Torres16.h" 36 | 37 | /** 38 | * @struct Direction 39 | * @brief Storage for line sweep direction 40 | * 41 | * Sweep direction is from edge to vertex 42 | */ 43 | struct Direction 44 | { 45 | LineSegment baseEdge; 46 | geometry_msgs::Point opposedVertex; 47 | }; 48 | 49 | /** 50 | * @brief Checks if given path is clockwise (the first turn made to the left) or not 51 | * @param path 52 | * @return bool True if path is clockwise 53 | * @details the definition of "clockwise" is based on Fig.8 in Torres et al. 2016 54 | */ 55 | inline bool isClockWise(const PointVector& path) 56 | { 57 | return path.at(0).x < path.at(1).x ? true : false; 58 | } 59 | 60 | /** 61 | * @brief Calculates line sweep direction for given polygon 62 | * @param polygon Line sweep direction is calculated on this region 63 | * @return direction Struct containing edge and vertex 64 | */ 65 | Direction identifyOptimalSweepDir(const PointVector& polygon) 66 | { 67 | Direction sweepDirection; 68 | 69 | PointVector convexHull = computeConvexHull(polygon); 70 | 71 | // Edges of polygon 72 | LineSegmentVector edges; 73 | 74 | // Make a list of edges of polygon 75 | for (std::size_t i = 0; i < convexHull.size(); ++i) 76 | { 77 | LineSegment ar; 78 | 79 | ar.at(0) = convexHull.at(i); 80 | 81 | // if vertex is the last one, 82 | // that vertex makes an edge whose end is the first vertex 83 | if (i == convexHull.size() - 1) 84 | { 85 | ar.at(1) = convexHull.at(0); 86 | } 87 | else 88 | { 89 | ar.at(1) = convexHull.at(i + 1); 90 | } 91 | edges.push_back(ar); 92 | } 93 | 94 | double optimalDistance = 0; 95 | 96 | // Calculate line sweep direction 97 | // Algorithm 1 in Torres et al, 2016 98 | for (const auto& edge : edges | boost::adaptors::indexed()) 99 | { 100 | double edgeMaxDistance = 0; 101 | geometry_msgs::Point opposedVertex; 102 | 103 | for (const geometry_msgs::Point& vertex : convexHull) 104 | { 105 | // calculateDistance() function returns distance 106 | // between given edge and vertex 107 | double distance = calculateDistance(edge.value(), vertex); 108 | 109 | if (distance > edgeMaxDistance) 110 | { 111 | edgeMaxDistance = distance; 112 | opposedVertex = vertex; 113 | } 114 | } 115 | 116 | if ((edgeMaxDistance < optimalDistance) or edge.index() == 0) 117 | { 118 | optimalDistance = edgeMaxDistance; 119 | sweepDirection.baseEdge = edge.value(); 120 | sweepDirection.opposedVertex = opposedVertex; 121 | } 122 | } 123 | 124 | return sweepDirection; 125 | } 126 | 127 | /** 128 | * @brief Reshape given path 129 | * @param path The sweep lines of path should be horizontal about x axis 130 | * @param padding 131 | * @return PointVector 132 | * @details Reshape given path so that generated path becomes the sequence of "C" shapes and add padding 133 | */ 134 | PointVector reshapePath(const PointVector& path, double padding) 135 | { 136 | PointVector zigzagPath; 137 | 138 | // reshape every traverse 139 | for (int i = 0; i < std::round(path.size() / 2); ++i) 140 | { 141 | // even-numbered traverse 142 | if (i % 2 == 0) 143 | { 144 | try 145 | { 146 | // in case that the first point of the traverse is located on LEFT side 147 | if (path.at(2 * i).x < path.at(2 * i + 1).x) 148 | { 149 | geometry_msgs::Point p1 = path.at(2 * i); 150 | geometry_msgs::Point p2 = path.at(2 * i + 1); 151 | 152 | // add padding 153 | p1.x += padding; 154 | p2.x -= padding; 155 | 156 | // be careful with the order of points 157 | zigzagPath.push_back(p1); 158 | zigzagPath.push_back(p2); 159 | } 160 | // in case that the first point of the traverse is located on RIGHT side 161 | else 162 | { 163 | geometry_msgs::Point p1 = path.at(2 * i); 164 | geometry_msgs::Point p2 = path.at(2 * i + 1); 165 | 166 | // add padding 167 | p1.x -= padding; 168 | p2.x += padding; 169 | 170 | // be careful with the order of points 171 | zigzagPath.push_back(p2); 172 | zigzagPath.push_back(p1); 173 | } 174 | } 175 | // in case that the traverse has only one vertex 176 | catch (std::out_of_range& ex) 177 | { 178 | geometry_msgs::Point p = path.at(2 * i); 179 | if (isClockWise(path)) 180 | { 181 | // the first vertex of even-numbered traverse of clockwise path is located on RIGHT side of polygon 182 | p.x += padding; 183 | zigzagPath.push_back(p); 184 | } 185 | else 186 | { 187 | // the first vertex of even-numbered traverse of counterclockwise path is located on LEFT side of polygon 188 | p.x -= padding; 189 | zigzagPath.push_back(p); 190 | } 191 | ROS_ERROR("%s", ex.what()); 192 | } 193 | } 194 | // odd-numbered traverse 195 | else 196 | { 197 | try 198 | { 199 | // in case that the first point of the traverse is located on RIGHT side 200 | if (path.at(2 * i).x > path.at(2 * i + 1).x) 201 | { 202 | geometry_msgs::Point p1 = path.at(2 * i); 203 | geometry_msgs::Point p2 = path.at(2 * i + 1); 204 | 205 | // add padding 206 | p1.x -= padding; 207 | p2.x += padding; 208 | 209 | // be careful with the order of points 210 | zigzagPath.push_back(p1); 211 | zigzagPath.push_back(p2); 212 | } 213 | // in case that the first point of the traverse is located on LEFT side 214 | else 215 | { 216 | geometry_msgs::Point p1 = path.at(2 * i); 217 | geometry_msgs::Point p2 = path.at(2 * i + 1); 218 | 219 | // add padding 220 | p1.x += padding; 221 | p2.x -= padding; 222 | 223 | // be careful with the order of points 224 | zigzagPath.push_back(p2); 225 | zigzagPath.push_back(p1); 226 | } 227 | } 228 | // in case that the traverse has only one vertex 229 | catch (std::out_of_range& ex) 230 | { 231 | geometry_msgs::Point p = path.at(2 * i); 232 | if (isClockWise(path)) 233 | { 234 | // the first vertex of odd-numbered traverse of clockwise path is located on LEFT side of polygon 235 | p.x -= padding; 236 | zigzagPath.push_back(p); 237 | } 238 | else 239 | { 240 | // the first vertex of odd-numbered traverse of clockwise path is located on RIGHT side of polygon 241 | p.x += padding; 242 | zigzagPath.push_back(p); 243 | } 244 | ROS_ERROR("%s", ex.what()); 245 | } 246 | } 247 | } 248 | return zigzagPath; 249 | } 250 | 251 | /** 252 | * @brief Compute coverage path for convex polygon 253 | * @param polygon Coverage path is calculated on this region 254 | * @param footprintWidth Width of the area taken by one sweep 255 | * @param horizontalOverwrap Horizontal overwrap of each sweep 256 | * @param sweepDirection 257 | * @param path Path of coverage path 258 | * @return bool True if path does not intersect with polygon 259 | */ 260 | bool computeConvexCoverage(const PointVector& polygon, double footprintWidth, double horizontalOverwrap, 261 | const Direction& sweepDirection, PointVector& path) 262 | { 263 | // Unable to make polygon with less than 3 points 264 | if (polygon.size() < 3) 265 | { 266 | return false; 267 | } 268 | 269 | // TODO: Change to configurable 270 | const double padding = footprintWidth/2.0; 271 | 272 | // rotate input polygon so that baseEdge become horizontal 273 | double rotationAngle = calculateHorizontalAngle(sweepDirection.baseEdge.front(), sweepDirection.baseEdge.back()); 274 | PointVector rotatedPolygon = rotatePoints(polygon, -rotationAngle); 275 | 276 | // find x coordinate of most left and most right point 277 | double minX(0), maxX(0); 278 | for (const auto& vertex : rotatedPolygon) 279 | { 280 | if (vertex.x < minX) 281 | { 282 | minX = vertex.x; 283 | } 284 | else if (vertex.x > maxX) 285 | { 286 | maxX = vertex.x; 287 | } 288 | } 289 | 290 | double stepWidth = footprintWidth * (1 - horizontalOverwrap); 291 | 292 | // calculate sweep direction of rotated polygon 293 | PointVector dir{ sweepDirection.opposedVertex, sweepDirection.baseEdge.front(), sweepDirection.baseEdge.back() }; 294 | dir = rotatePoints(dir, -rotationAngle); 295 | Direction rotatedDir; 296 | rotatedDir.opposedVertex = dir.at(0); 297 | rotatedDir.baseEdge.front() = dir.at(1); 298 | rotatedDir.baseEdge.back() = dir.at(2); 299 | 300 | int stepNum = std::ceil(calculateDistance(rotatedDir.baseEdge, rotatedDir.opposedVertex) / stepWidth); 301 | 302 | LineSegmentVector sweepLines; 303 | 304 | // generate list of sweep lines which is horizontal against the base edge 305 | for (int i = 0; i < stepNum; ++i) 306 | { 307 | LineSegment ar; 308 | geometry_msgs::Point p1, p2; 309 | p1.x = minX; 310 | p1.y = rotatedDir.baseEdge.at(0).y + (i * stepWidth) + padding; 311 | p2.x = maxX; 312 | p2.y = rotatedDir.baseEdge.at(1).y + (i * stepWidth) + padding; 313 | 314 | ar.at(0) = p1; 315 | ar.at(1) = p2; 316 | 317 | sweepLines.push_back(ar); 318 | } 319 | 320 | // Localize intersections of sweeplines and edges of rotated polygon 321 | LineSegmentVector rotatedEdges = generateEdgeVector(rotatedPolygon, true); 322 | 323 | PointVector intersections; 324 | 325 | for (const auto& sweepLine : sweepLines) 326 | { 327 | int intersectionCount = 0; 328 | for (const auto& edge : rotatedEdges) 329 | { 330 | if (hasIntersection(sweepLine, edge)) 331 | { 332 | intersections.push_back(localizeIntersection(edge, sweepLine)); 333 | ++intersectionCount; 334 | } 335 | 336 | // sweep line in optimal path does not have more than 2 intersections 337 | if (intersectionCount >= 3) 338 | { 339 | return false; 340 | } 341 | } 342 | } 343 | 344 | // sort points by y coordinate in ascending order 345 | std::stable_sort(intersections.begin(), intersections.end(), 346 | [](const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { return p1.y < p2.y; }); 347 | 348 | PointVector rotatedPath = reshapePath(intersections, padding); 349 | 350 | path = rotatePoints(rotatedPath, rotationAngle); 351 | 352 | if (hasIntersection(generateEdgeVector(polygon, true), generateEdgeVector(path, false)) == true) 353 | { 354 | return false; 355 | } 356 | 357 | return true; 358 | } 359 | 360 | /** 361 | * @brief Compute coverage path for convex polygon 362 | * @param polygon Coverage path is calculated on this region 363 | * @param footprintWidth Width of the area taken by one sweep 364 | * @param horizontalOverwrap Horizontal overwrap of each sweep 365 | * @param path Path of coverage path 366 | * @return bool True if path does not intersect with polygon 367 | */ 368 | bool computeConvexCoverage(const PointVector& polygon, double footprintWidth, double horizontalOverwrap, 369 | PointVector& path) 370 | { 371 | Direction sweepDirection = identifyOptimalSweepDir(polygon); 372 | return computeConvexCoverage(polygon, footprintWidth, horizontalOverwrap, sweepDirection, path); 373 | } 374 | 375 | /** 376 | * @brief Calculates length of path 377 | * @param path 378 | * @return double Length of path 379 | */ 380 | double calculatePathLength(const PointVector& path) 381 | { 382 | if (path.size() < 2) 383 | { 384 | return 0; 385 | } 386 | 387 | double pathLength = 0; 388 | for (int i = 0; i < path.size() - 1; ++i) 389 | { 390 | pathLength += calculateDistance(path.at(i), path.at(i + 1)); 391 | } 392 | return pathLength; 393 | } 394 | 395 | /** 396 | * @brief Return counter clock wise-ed path of given path 397 | * @param path Clockwise path 398 | * @return PointVector Counter clock wise version of given path 399 | */ 400 | PointVector computeCCWPath(PointVector path) 401 | { 402 | for (int i = 0; i < std::round(path.size() / 2); ++i) 403 | { 404 | // swap the first point and the last point in each sweep line 405 | geometry_msgs::Point tmp = path.at(2 * i); 406 | 407 | path.at(2 * i) = path.at(2 * i + 1); 408 | try 409 | { 410 | path.at(2 * i + 1) = tmp; 411 | } 412 | catch (std::out_of_range& ex) 413 | { 414 | ROS_ERROR("%s", ex.what()); 415 | } 416 | } 417 | return path; 418 | } 419 | 420 | /** 421 | * @brief Return opposite path of given path 422 | * @param path 423 | * @return PointVector Path with points of reversed order of given path 424 | */ 425 | PointVector computeOppositePath(const PointVector& path) 426 | { 427 | PointVector oppositePath; 428 | 429 | // inversely iterate given points 430 | for (int i = path.size() - 1; i >= 0; --i) 431 | { 432 | oppositePath.push_back(path.at(i)); 433 | } 434 | 435 | return oppositePath; 436 | } 437 | 438 | /** 439 | * @brief Identify optimal path from 4 coverage alternatives 440 | * @param polygon 441 | * @param path 442 | * @param start Start point 443 | * @param end End point 444 | * @return PointVector Optimal path that minimizes the length of path 445 | * @details The naming of the following variable follows torres et al. 2016 446 | */ 447 | PointVector identifyOptimalAlternative(const PointVector& polygon, const PointVector& path, 448 | const geometry_msgs::Point& start, const geometry_msgs::Point& end) 449 | { 450 | // The naming of the following variable follows torres et al. 2016 451 | std::unordered_map> coverageAlternatives; 452 | std::unordered_map a1, a2, a3, a4; 453 | 454 | PointVector pathCW = isClockWise(path) ? path : computeCCWPath(path); 455 | PointVector pathCCW = isClockWise(path) ? computeCCWPath(path) : path; 456 | 457 | // a1: clockwise current path 458 | a1["SP"] = pathCW.front(); 459 | a1["EP"] = pathCW.back(); 460 | 461 | // a2: counterclockwise current path 462 | a2["SP"] = pathCCW.front(); 463 | a2["EP"] = pathCCW.back(); 464 | 465 | // a3: clockwise opposite path 466 | a3["SP"] = pathCW.back(); 467 | a3["EP"] = pathCW.front(); 468 | 469 | // a4: counterclockwise opposite path 470 | a4["SP"] = pathCCW.back(); 471 | a4["EP"] = pathCCW.front(); 472 | 473 | coverageAlternatives[1] = a1; 474 | coverageAlternatives[2] = a2; 475 | coverageAlternatives[3] = a3; 476 | coverageAlternatives[4] = a4; 477 | 478 | bool hasIntersectionCW = hasIntersection(generateEdgeVector(polygon, true), generateEdgeVector(pathCW, false)); 479 | bool hasIntersectionCCW = hasIntersection(generateEdgeVector(polygon, true), generateEdgeVector(pathCCW, false)); 480 | 481 | double minDistance; 482 | int optimalPath; 483 | 484 | // check which coverage alternative has the shortest path 485 | for (const auto& coverage : coverageAlternatives | boost::adaptors::indexed()) 486 | { 487 | // skip calculating length if the path has intersections 488 | if ((hasIntersectionCW and coverage.index() % 2 == 0) or (hasIntersectionCCW and coverage.index() % 2 != 0)) 489 | { 490 | continue; 491 | } 492 | 493 | // only length of transition need to be considered because the length of coverage is almost same 494 | double distance = calculateDistance(coverage.value().second.at("SP"), start) + 495 | calculateDistance(end, coverage.value().second.at("EP")); 496 | 497 | if (distance < minDistance or coverage.index() == 0) 498 | { 499 | minDistance = distance; 500 | optimalPath = coverage.value().first; 501 | } 502 | } 503 | 504 | switch (optimalPath) 505 | { 506 | case 1: 507 | { 508 | return pathCW; 509 | } 510 | case 2: 511 | { 512 | return pathCCW; 513 | } 514 | case 3: 515 | { 516 | return computeOppositePath(pathCW); 517 | } 518 | default: 519 | { 520 | return computeOppositePath(pathCCW); 521 | } 522 | } 523 | } 524 | 525 | /** 526 | * @brief Identify optimal path from 4 coverage alternatives 527 | * @param polygon 528 | * @param path 529 | * @param start Start point 530 | * @return PointVector Optimal path that minimizes the length of path 531 | * @details The naming of the following variable follows torres et al. 2016 532 | */ 533 | PointVector identifyOptimalAlternative(const PointVector& polygon, const PointVector& path, 534 | const geometry_msgs::Point& start) 535 | { 536 | return identifyOptimalAlternative(polygon, path, start, start); 537 | } 538 | 539 | /** 540 | * @brief Find second optimal path 541 | * @param polygon 542 | * @param footprintWidth 543 | * @param horizontalOverwrap 544 | * @param path 545 | * @return bool True if second optimal path exists 546 | */ 547 | bool findSecondOptimalPath(const PointVector& polygon, double footprintWidth, double horizontalOverwrap, 548 | PointVector& path) 549 | { 550 | std::vector sweepDirections; 551 | PointVector convexHull = computeConvexHull(polygon); 552 | LineSegmentVector edges = generateEdgeVector(convexHull, true); 553 | 554 | // compute optimal sweep directions for each edge 555 | for (const auto& edge : edges) 556 | { 557 | double maxDistance = 0; 558 | Direction direction; 559 | direction.baseEdge = edge; 560 | for (const auto& vertex : convexHull) 561 | { 562 | double distance = calculateDistance(edge, vertex); 563 | 564 | // optimal sweep direction for a edge is the direction with the largest distance 565 | if (distance > maxDistance) 566 | { 567 | maxDistance = distance; 568 | direction.opposedVertex = vertex; 569 | } 570 | } 571 | sweepDirections.push_back(direction); 572 | } 573 | 574 | // compute second optimal path which has the shortest coverage path 575 | double pathLength = 0; 576 | PointVector tempPath; 577 | for (const auto& sweepDirection : sweepDirections) 578 | { 579 | PointVector p; 580 | 581 | // isValidPath is true if computed coverage does not have intersection 582 | bool isValidPath = computeConvexCoverage(polygon, footprintWidth, horizontalOverwrap, sweepDirection, p); 583 | 584 | // second optimal path is the shortest path without intersection 585 | if (isValidPath and ((calculatePathLength(tempPath) < pathLength) or (pathLength == 0))) 586 | { 587 | tempPath = p; 588 | pathLength = calculatePathLength(tempPath); 589 | } 590 | } 591 | 592 | if (tempPath.size() <= 1) 593 | { 594 | return false; 595 | } 596 | else 597 | { 598 | path = tempPath; 599 | return true; 600 | } 601 | } 602 | 603 | /** 604 | * @brief Check if given two polygons are adjacent 605 | * @param polygon1 606 | * @param polygon2 607 | * @return True if given two polygons are adjacent 608 | */ 609 | bool isAdjacent(const PointVector& polygon1, const PointVector& polygon2) 610 | { 611 | for (const auto& vertex1 : polygon1) 612 | { 613 | for (const auto& vertex2 : polygon2) 614 | { 615 | // consider that two polygons are adjacent if they have at least one point in common 616 | if (vertex1 == vertex2) 617 | { 618 | return true; 619 | } 620 | } 621 | } 622 | return false; 623 | } 624 | 625 | /** 626 | * @brief Compute coverage path for multiple convex polygons 627 | * @param subPolygons 628 | * @param footprintWidth 629 | * @param horizontalOverwrap 630 | * @param adjacencyCriteria Ignore paths which have less adjacent polygons than this number 631 | * @return PointVector Computed Path 632 | * @details See section 6.2 of torres et al. 2016 for the detail 633 | */ 634 | PointVector computeMultiplePolygonCoverage(std::vector subPolygons, double footprintWidth, 635 | double horizontalOverwrap, int adjacencyCriteria = 1) 636 | { 637 | PointVector path; 638 | 639 | std::vector permutation(subPolygons.size()); 640 | std::iota(permutation.begin(), permutation.end(), 0); 641 | 642 | double minPathLength = -1; 643 | bool hasIntersection = false; 644 | 645 | int numPermutation = 0; 646 | int numIntersectSets = 0; 647 | 648 | do 649 | { 650 | // ignore permutations which do not start from the same polygon as the first one of given subpolygons 651 | if (permutation.front() != 0) 652 | { 653 | continue; 654 | } 655 | 656 | // count adjacent polygons 657 | int adjacencyCount = 0; 658 | for (auto itr = permutation.begin(); itr != permutation.end() - 1; ++itr) 659 | { 660 | if (isAdjacent(subPolygons.at(*itr), subPolygons.at(*(itr + 1))) == true) 661 | { 662 | ++adjacencyCount; 663 | } 664 | } 665 | 666 | // ignore if enough number of polygons do not exist 667 | if (adjacencyCount < adjacencyCriteria) 668 | { 669 | continue; 670 | } 671 | 672 | double pathLength = 0; 673 | std::vector candidatePath; 674 | 675 | // computes coverage path for each polygons and connect them 676 | for (auto itr = permutation.begin(); itr != permutation.end(); ++itr) 677 | { 678 | PointVector partPath, optimalAlternative; 679 | // first polygon of given subpolygon 680 | if (itr == permutation.begin()) 681 | { 682 | try 683 | { 684 | // start point and end point of first subpolygon are ... 685 | // start point: the last point of coverage of the last subpolygon 686 | // end point : the first point of coverage of the second subpolygon 687 | geometry_msgs::Point start = subPolygons.at(*(permutation.end() - 1)).back(); 688 | geometry_msgs::Point end; 689 | PointVector polygon = subPolygons.at(*itr); 690 | if (permutation.size() == 1) 691 | { 692 | // end point is the same as start point if subPolygons.at(*(itr+1)) is out of range 693 | end = start; 694 | } 695 | else 696 | { 697 | end = subPolygons.at(*(itr + 1)).front(); 698 | } 699 | 700 | // break if computed path has intersections 701 | if (computeConvexCoverage(polygon, footprintWidth, horizontalOverwrap, partPath) == false) 702 | { 703 | hasIntersection = true; 704 | break; 705 | } 706 | 707 | // set optimal alternative as candidate 708 | optimalAlternative = identifyOptimalAlternative(polygon, partPath, start, end); 709 | candidatePath.push_back(optimalAlternative); 710 | 711 | // calculate the length of candidate path 712 | pathLength += calculatePathLength(optimalAlternative); 713 | } 714 | catch (std::out_of_range& ex) 715 | { 716 | ROS_ERROR("%s", ex.what()); 717 | } 718 | } 719 | // the last polygon of the given subpolygons 720 | else if (itr == permutation.end() - 1) 721 | { 722 | try 723 | { 724 | // start point and end point of the last subpolygon are ... 725 | // start point: the last point of coverage of the previous subpolygon 726 | // end point : the first point of coverage of the first subpolygon 727 | geometry_msgs::Point start = subPolygons.at(*(itr - 1)).back(); 728 | geometry_msgs::Point end = subPolygons.at(*permutation.begin()).front(); 729 | PointVector polygon = subPolygons.at(*itr); 730 | 731 | // break if computed path has intersections 732 | if (computeConvexCoverage(polygon, footprintWidth, horizontalOverwrap, partPath) == false) 733 | { 734 | hasIntersection = true; 735 | break; 736 | } 737 | 738 | // set optimal alternative as candidate 739 | optimalAlternative = identifyOptimalAlternative(polygon, partPath, start, end); 740 | candidatePath.push_back(optimalAlternative); 741 | 742 | // calculate the length of candidate path 743 | pathLength += calculatePathLength(optimalAlternative); 744 | } 745 | catch (std::out_of_range& ex) 746 | { 747 | ROS_ERROR("%s", ex.what()); 748 | } 749 | } 750 | // middle polygons 751 | else 752 | { 753 | try 754 | { 755 | // start point and end point of middle subpolygons are ... 756 | // start point: the last point of coverage of the previous subpolygon 757 | // end point : the first point of coverage of the next subpolygon 758 | geometry_msgs::Point start = subPolygons.at(*(itr - 1)).back(); 759 | geometry_msgs::Point end = subPolygons.at(*(itr + 1)).front(); 760 | PointVector polygon = subPolygons.at(*itr); 761 | 762 | // break if computed path has intersections 763 | if (computeConvexCoverage(polygon, footprintWidth, horizontalOverwrap, partPath) == false) 764 | { 765 | hasIntersection = true; 766 | break; 767 | } 768 | 769 | // set optimal alternative as candidate 770 | optimalAlternative = identifyOptimalAlternative(polygon, partPath, start, end); 771 | candidatePath.push_back(optimalAlternative); 772 | 773 | // calculate the length of candidate path 774 | pathLength += calculatePathLength(optimalAlternative); 775 | } 776 | catch (std::out_of_range& ex) 777 | { 778 | ROS_ERROR("%s", ex.what()); 779 | } 780 | } 781 | } 782 | 783 | numPermutation++; 784 | 785 | if(hasIntersection) 786 | { 787 | numIntersectSets++; 788 | hasIntersection = false; 789 | break; 790 | } 791 | 792 | // update coverage path and minimum path length 793 | if (minPathLength < 0 or pathLength < minPathLength) 794 | { 795 | minPathLength = pathLength; 796 | 797 | if (not path.empty()) 798 | { 799 | path.clear(); 800 | } 801 | 802 | // insert coverages of subpolygons 803 | for (const auto& part : candidatePath) 804 | { 805 | path.insert(path.begin(), part.begin(), part.end()); 806 | } 807 | } 808 | 809 | } while (next_permutation(permutation.begin(), permutation.end())); 810 | 811 | if(minPathLength<0) 812 | { 813 | ROS_ERROR("Unable to generate path."); 814 | } 815 | 816 | return path; 817 | } 818 | 819 | #endif 820 | -------------------------------------------------------------------------------- /launch/cpp_uav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cpp_uav 4 | 0.1.0 5 | The package for aerial coverage path planning. 6 | 7 | Takaki Ueno 8 | 9 | MIT 10 | https://github.com/uenota/cpp_uav 11 | https://github.com/uenota/cpp_uav/issues 12 | 13 | catkin 14 | 15 | roslint 16 | geometry_msgs 17 | roscpp 18 | rospy 19 | std_msgs 20 | message_generation 21 | cgal 22 | cgal-qt5-dev 23 | doxygen 24 | 25 | geometry_msgs 26 | roscpp 27 | rospy 28 | std_msgs 29 | message_runtime 30 | 31 | python-matplotlib 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /script/specify_rect.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # Copyright (c) 2017 Takaki Ueno 4 | # Released under the MIT license 5 | 6 | """! @package cpp_uav 7 | This module offers GUI to specify a polygon for coverage path planning. 8 | """ 9 | 10 | # Import python3's print to suppress warning raised by pylint 11 | from __future__ import print_function 12 | 13 | # python libraries 14 | from math import tan 15 | 16 | import numpy as np 17 | 18 | # rospy 19 | import rospy 20 | 21 | # messages 22 | from geometry_msgs.msg import Point 23 | from std_msgs.msg import Float64 24 | 25 | # service 26 | from cpp_uav.srv import Torres16 27 | 28 | # Check maplotlib's version 29 | # Exit if older than 2.1.0 30 | import matplotlib 31 | if matplotlib.__version__ >= "2.1.0": 32 | # matplotlib 33 | # matplotlib 2.1.0 or newer is required to import TextBox 34 | from matplotlib import pyplot as plt 35 | from matplotlib.patches import Polygon 36 | from matplotlib.widgets import Button 37 | from matplotlib.widgets import TextBox 38 | else: 39 | print("Matplotlib 2.1.0 or newer is required to run this node.") 40 | print("Please update or install Matplotlib.") 41 | exit(1) 42 | 43 | 44 | class PolygonBuilder(object): 45 | """! 46 | GUI class to specify a polygon 47 | """ 48 | 49 | def __init__(self): 50 | """! Constructor 51 | """ 52 | 53 | # @var fig 54 | # Figure instance 55 | self.fig = plt.figure(figsize=(10, 8)) 56 | 57 | # @var axis 58 | # Axis object where polygon is shown 59 | self.axis = self.fig.add_subplot(111) 60 | 61 | # adjust the size of graph 62 | self.axis.set_ylim([-100, 100]) 63 | self.axis.set_xlim([-100, 100]) 64 | 65 | # set aspect ratio so that aspect ration of graph become 1 66 | self.axis.set_aspect('equal', adjustable="box") 67 | 68 | self.fig.subplots_adjust(top=0.95, bottom=0.35, right=0.79) 69 | 70 | # @var is_polygon_drawn 71 | # True if polygon is illustrated on window 72 | self.is_polygon_drawn = False 73 | 74 | # @var is_path_drawn 75 | # True if path is illustrated on window 76 | self.is_path_drawn = False 77 | 78 | # @var server_node 79 | # Instance of server 80 | self.server_node = None 81 | 82 | # @var lines 83 | # Dictionary to store Line2D objects 84 | # - line: Line2D object representing edges of a polygon 85 | # - dot: Line2D object representing vertices of a polygon 86 | # - path: Line2D object representing coverage path 87 | self.lines = {"line": self.axis.plot([], [], "-")[0], 88 | "dot": self.axis.plot([], [], "o")[0], 89 | "path": self.axis.plot([], [], "-")[0], 90 | "subpolygons": None} 91 | 92 | # Register __call__ as callback function for line and dot 93 | self.lines["line"].figure.canvas.mpl_connect( 94 | 'button_press_event', self) 95 | self.lines["dot"].figure.canvas.mpl_connect('button_press_event', self) 96 | 97 | # @var points 98 | # Dictionary to store points 99 | # - vertices_x: List of x coordinates of vertices 100 | # - vertices_y: List of y coordinates of vertices 101 | # - start: geometry_msgs/Point object stores info about start point 102 | # - waypoints: List of waypoints returned by a coverage path planner 103 | self.points = {"vertices_x": list(), 104 | "vertices_y": list(), 105 | "start": None, 106 | "waypoints": list()} 107 | 108 | self.subpolygons = [] 109 | self.patches = [] 110 | 111 | # @var shooting_cond 112 | # Dictionary of shooting condition 113 | # - image_resolution_h [px]: Vertical resolution of image 114 | # - image_resolution_w [px]: Horizontal resolution of image 115 | # - angle_of_view [deg]: Camera's angle of view 116 | # - height [m]: Flight height 117 | self.shooting_cond = {"image_resolution_h": 640, 118 | "image_resolution_w": 320, 119 | "angle_of_view": 45.0, 120 | "height": 30.0} 121 | 122 | footprint_width = Float64(2 * self.shooting_cond["height"] * 123 | tan(self.shooting_cond["angle_of_view"] / 2)) 124 | aspect_ratio = \ 125 | float(self.shooting_cond["image_resolution_w"]) \ 126 | / self.shooting_cond["image_resolution_h"] 127 | 128 | # @var coverage_params 129 | # Dictionary of coverage params 130 | # - footprint_width [m]: Width of footprint 131 | # - footprint_length [m]: Length of footprint 132 | # - aspect_ratio []: Aspect of image 133 | # - horizontal_overwrap [%]: Horizontal overwrap of footprint 134 | # - vertical_overwrap [%]: Vertical overwrap of footprint 135 | # cf. torres et, al. 2016 136 | self.coverage_params = {"footprint_width": 137 | footprint_width, 138 | "footprint_length": 139 | Float64(footprint_width.data * 140 | aspect_ratio), 141 | "aspect_ratio": 142 | aspect_ratio, 143 | "horizontal_overwrap": Float64(0.3), 144 | "vertical_overwrap": Float64(0.2)} 145 | 146 | # @var buttons 147 | # Dictionary of buttons 148 | # - draw_button: Button object to evoke draw_polygon method 149 | # - calc_button: Button object to evoke calculate_path method 150 | # - clear_button: Button object to evoke clear_figure method 151 | self.buttons = {"draw_button": 152 | Button(plt.axes([0.8, 0.80, 0.15, 0.075]), 153 | 'Draw Polygon'), 154 | "calc_button": 155 | Button(plt.axes([0.8, 0.69, 0.15, 0.075]), 156 | 'Calculate CP'), 157 | "clear_button": 158 | Button(plt.axes([0.8, 0.58, 0.15, 0.075]), 159 | 'Clear')} 160 | 161 | # Register callback functions for buttons 162 | self.buttons["draw_button"].on_clicked(self.draw_polygon) 163 | self.buttons["calc_button"].on_clicked(self.calculate_path) 164 | self.buttons["clear_button"].on_clicked(self.clear_figure) 165 | 166 | # Create textboxes 167 | # @var text_boxes 168 | # Dictionary of text boxes 169 | # - image_resolution_h_box: TextBox object to get input for image_resolution_h 170 | # - image_resolution_w_box: TextBox object to get input for image_resolution_w 171 | # - angle_of_view_box: TextBox object to get input for angle_of_view 172 | # - height_box: TextBox object to get input for height 173 | # - horizontal_overwrap_box: TextBox object to get input for horizontal_overwrap 174 | # - vertical_overwrap_box: TextBox object to get input for vertical_overwrap 175 | self.text_boxes = {"image_resolution_h_box": 176 | TextBox(plt.axes([0.25, 0.25, 0.1, 0.05]), 177 | "Image Height [px]", 178 | initial=str(self.shooting_cond["image_resolution_h"])), 179 | "image_resolution_w_box": 180 | TextBox(plt.axes([0.6, 0.25, 0.1, 0.05]), 181 | "Image Width [px]", 182 | initial=str(self.shooting_cond["image_resolution_w"])), 183 | "angle_of_view_box": 184 | TextBox(plt.axes([0.25, 0.175, 0.1, 0.05]), 185 | "Angle of view [deg]", 186 | initial=str(self.shooting_cond["angle_of_view"])), 187 | "height_box": 188 | TextBox(plt.axes([0.6, 0.175, 0.1, 0.05]), 189 | "Height [m]", 190 | initial=str(self.shooting_cond["height"])), 191 | "horizontal_overwrap_box": 192 | TextBox(plt.axes([0.25, 0.1, 0.1, 0.05]), 193 | "Horizontal Overwrap [0.0 - 1.0]", 194 | initial=str(self.coverage_params["horizontal_overwrap"].data)), 195 | "vertical_overwrap_box": 196 | TextBox(plt.axes([0.6, 0.1, 0.1, 0.05]), 197 | "Vertical Overwrap [0.0 - 1.0]", 198 | initial=str(self.coverage_params["vertical_overwrap"].data))} 199 | 200 | # Register callback functions for textboxes 201 | self.text_boxes["image_resolution_h_box"].on_submit( 202 | self.image_resolution_h_update) 203 | self.text_boxes["image_resolution_w_box"].on_submit( 204 | self.image_resolution_w_update) 205 | self.text_boxes["angle_of_view_box"].on_submit( 206 | self.angle_of_view_update) 207 | self.text_boxes["height_box"].on_submit(self.height_update) 208 | self.text_boxes["horizontal_overwrap_box"].on_submit( 209 | self.horizontal_overwrap_update) 210 | self.text_boxes["vertical_overwrap_box"].on_submit( 211 | self.vertical_overwrap_update) 212 | 213 | # @var labels 214 | # Dictionary of labels on figure 215 | # - aspect_ratio_text: Text object to display aspect_ratio 216 | # - footprint_length_text: Text object to display footprint_length 217 | # - footprint_width_text: Text object to display footprint_width 218 | # - mode_text: Text object to display mode 219 | # - start_point: Text object to display start point 220 | # - SP: Start of waypoints 221 | # - EP: End of waypoints 222 | self.labels = {"aspect_ratio_text": 223 | plt.text(2, 5, 224 | "Aspect ratio:\n " + str(self.coverage_params["aspect_ratio"])), 225 | "footprint_length_text": 226 | plt.text(2, 4, 227 | "Footprint Length [m]:\n " + 228 | str(round(self.coverage_params["footprint_length"].data, 2))), 229 | "footprint_width_text": 230 | plt.text(2, 3, 231 | "Footprint Width [m]:\n " + 232 | str(round(self.coverage_params["footprint_width"].data, 2))), 233 | "start_point": None, 234 | "SP": None, 235 | "EP": None} 236 | 237 | # plot a figure 238 | plt.show() 239 | 240 | def __call__(self, event): 241 | """! 242 | Callback function for button_press_event 243 | @param event MouseEvent object 244 | """ 245 | # Return if click event doesn't happen in same axis as which a line lies on 246 | if event.inaxes != self.lines["line"].axes: 247 | return 248 | # true if polygon is not drawn 249 | if not self.is_polygon_drawn: 250 | self.points["vertices_x"].append(event.xdata) 251 | self.points["vertices_y"].append(event.ydata) 252 | # illustrate a dot 253 | self.lines["dot"].set_data(self.points["vertices_x"], 254 | self.points["vertices_y"]) 255 | self.lines["dot"].figure.canvas.draw() 256 | # true if start point is not set 257 | elif not self.points["start"]: 258 | # set start point 259 | self.points["start"] = Point() 260 | self.points["start"].x = event.xdata 261 | self.points["start"].y = event.ydata 262 | 263 | # set and display start point and its label 264 | self.labels["start_point"] = self.axis.text( 265 | event.xdata, event.ydata, "Start", color="red", fontsize=16) 266 | self.lines["dot"].set_data(self.points["vertices_x"] + [event.xdata], 267 | self.points["vertices_y"] + [event.ydata]) 268 | self.lines["dot"].figure.canvas.draw() 269 | else: 270 | rospy.logwarn("Unable to register points anymore.") 271 | 272 | def update_params(self): 273 | """! 274 | Update values of coverage parameters 275 | """ 276 | self.coverage_params["aspect_ratio"] = float( 277 | self.shooting_cond["image_resolution_w"]) / self.shooting_cond["image_resolution_h"] 278 | self.coverage_params["footprint_width"] = Float64( 279 | 2 * self.shooting_cond["height"] * tan(self.shooting_cond["angle_of_view"] / 2)) 280 | self.coverage_params["footprint_length"] = Float64( 281 | self.coverage_params["footprint_width"].data / self.coverage_params["aspect_ratio"]) 282 | 283 | def update_param_texts(self): 284 | """! 285 | Update texts of coverage parameters 286 | """ 287 | self.labels["aspect_ratio_text"].set_text("Aspect ratio:\n " + 288 | str(self.coverage_params["aspect_ratio"])) 289 | self.labels["footprint_length_text"].set_text("Footprint Length [m]:\n " + 290 | str(round(self.coverage_params["footprint_length"].data, 2))) 291 | self.labels["footprint_width_text"].set_text("Footprint Width [m]:\n " + 292 | str(round(self.coverage_params["footprint_width"].data, 2))) 293 | self.labels["footprint_length_text"].figure.canvas.draw() 294 | 295 | def draw_polygon(self, event): 296 | """! 297 | Callback function for "Draw Polygon" button 298 | @param event MouseEvent object 299 | """ 300 | # Return if vertices is less than 3 301 | if len(self.points["vertices_x"]) < 3: 302 | rospy.logerr("Unable to make a polygon.") 303 | return 304 | # Draw a polygon 305 | self.lines["line"].set_data(self.points["vertices_x"] + self.points["vertices_x"][0:1], 306 | self.points["vertices_y"] + self.points["vertices_y"][0:1]) 307 | self.lines["line"].figure.canvas.draw() 308 | # Set flag as True 309 | self.is_polygon_drawn = True 310 | 311 | def calculate_path(self, event): 312 | """! 313 | Callback function for "Calculate CP" button 314 | @param event MouseEvent object 315 | """ 316 | if not self.is_polygon_drawn: 317 | return 318 | 319 | if not self.points["start"]: 320 | rospy.logwarn("Choose start point.") 321 | return 322 | 323 | if self.is_path_drawn: 324 | return 325 | 326 | # assign server node if server node is None 327 | if not self.server_node: 328 | rospy.loginfo("Waiting for Server Node.") 329 | try: 330 | rospy.wait_for_service("cpp_torres16", 331 | timeout=5.0) 332 | except rospy.ROSException: 333 | rospy.logerr("Server not found.") 334 | return 335 | try: 336 | self.server_node = rospy.ServiceProxy( 337 | "cpp_torres16", 338 | Torres16) 339 | except rospy.ServiceException as ex: 340 | rospy.logerr(str(ex)) 341 | return 342 | 343 | # Create a list of vertices 344 | vertices = [] 345 | waypoint_xs = [] 346 | waypoint_ys = [] 347 | 348 | # fill the list of vertices that is passed to server node 349 | for x_coord, y_coord in zip(self.points["vertices_x"], 350 | self.points["vertices_y"]): 351 | point = Point() 352 | point.x = x_coord 353 | point.y = y_coord 354 | vertices.append(point) 355 | 356 | # Call service 357 | try: 358 | ret = self.server_node(vertices, 359 | self.points["start"], 360 | self.coverage_params["footprint_length"], 361 | self.coverage_params["footprint_width"], 362 | self.coverage_params["horizontal_overwrap"], 363 | self.coverage_params["vertical_overwrap"]) 364 | self.points["waypoints"] = ret.path 365 | self.subpolygons = ret.subpolygons 366 | 367 | # fill the lists of waypoints' coordinate to draw path 368 | for num, point in enumerate(self.points["waypoints"]): 369 | if num == 0: 370 | waypoint_xs.append(self.points["start"].x) 371 | waypoint_ys.append(self.points["start"].y) 372 | self.labels["SP"] = self.axis.text( 373 | point.x, point.y, "SP", color="red", fontsize=16) 374 | waypoint_xs.append(point.x) 375 | waypoint_ys.append(point.y) 376 | if num == len(self.points["waypoints"]) - 1: 377 | waypoint_xs.append(self.points["start"].x) 378 | waypoint_ys.append(self.points["start"].y) 379 | self.labels["EP"] = self.axis.text( 380 | point.x, point.y, "EP", color="red", fontsize=16) 381 | for subpolygon in self.subpolygons: 382 | arr = np.ndarray([len(subpolygon.points), 2]) 383 | for num, point in enumerate(subpolygon.points): 384 | arr[num][0] = point.x 385 | arr[num][1] = point.y 386 | patch = Polygon(xy=arr, alpha=0.5, edgecolor="navy") 387 | self.axis.add_patch(patch) 388 | self.patches.append(patch) 389 | 390 | self.lines["path"].set_data(waypoint_xs, waypoint_ys) 391 | self.lines["path"].figure.canvas.draw() 392 | 393 | self.is_path_drawn = True 394 | except rospy.ServiceException as ex: 395 | rospy.logerr(str(ex)) 396 | return 397 | 398 | def clear_figure(self, event): 399 | """! 400 | Callback function for "Clear" button 401 | @param event MouseEvent object 402 | """ 403 | 404 | # Clear lists of vertices' coordinates 405 | self.points["vertices_x"] = [] 406 | self.points["vertices_y"] = [] 407 | # Set flag as False 408 | self.is_polygon_drawn = False 409 | self.is_path_drawn = False 410 | # Clear start point 411 | self.points["start"] = None 412 | 413 | # Clear waypoints 414 | self.points["waypoints"] = [] 415 | self.points["subpolygons"] = [] 416 | 417 | # Clear point data 418 | self.lines["dot"].set_data( 419 | self.points["vertices_x"], self.points["vertices_y"]) 420 | self.lines["line"].set_data( 421 | self.points["vertices_x"], self.points["vertices_y"]) 422 | self.lines["path"].set_data([], []) 423 | 424 | if self.labels["start_point"]: 425 | self.labels["start_point"].remove() 426 | if self.labels["SP"]: 427 | try: 428 | self.labels["SP"].remove() 429 | self.labels["EP"].remove() 430 | except ValueError: 431 | pass 432 | 433 | for patch in self.patches: 434 | patch.remove() 435 | 436 | self.patches = [] 437 | 438 | # Refresh a canvas 439 | self.lines["dot"].figure.canvas.draw() 440 | self.lines["line"].figure.canvas.draw() 441 | self.lines["path"].figure.canvas.draw() 442 | 443 | def image_resolution_h_update(self, event): 444 | """! 445 | Called when content of "Image Height" is submitted 446 | @param event Content of TextBox 447 | """ 448 | # Update parameter if input is digit 449 | try: 450 | self.shooting_cond["image_resolution_h"] = int(event) 451 | self.update_params() 452 | self.update_param_texts() 453 | except TypeError: 454 | self.text_boxes["image_resolution_h_box"].\ 455 | set_val(str(self.shooting_cond["image_resolution_h"])) 456 | 457 | def image_resolution_w_update(self, event): 458 | """! 459 | Called when content of "Image Width" is submitted 460 | @param event Content of TextBox 461 | """ 462 | # Update parameter if input is digit 463 | try: 464 | self.shooting_cond["image_resolution_w"] = int(event) 465 | self.update_params() 466 | self.update_param_texts() 467 | except TypeError: 468 | self.text_boxes["image_resolution_w_box"].\ 469 | set_val(str(self.shooting_cond["image_resolution_w"])) 470 | 471 | def angle_of_view_update(self, event): 472 | """! 473 | Called when content of "Angle of view" is submitted 474 | @param event Content of TextBox 475 | """ 476 | # Update parameter if input is digit 477 | try: 478 | self.shooting_cond["angle_of_view"] = float(event) 479 | self.update_params() 480 | self.update_param_texts() 481 | except TypeError: 482 | self.text_boxes["angle_of_view_box"]\ 483 | .set_val(str(self.shooting_cond["angle_of_view"])) 484 | 485 | def height_update(self, event): 486 | """! 487 | Called when content of "Height" is submitted 488 | @param event Content of TextBox 489 | """ 490 | # Update parameter if input is digit 491 | try: 492 | self.shooting_cond["height"] = float(event) 493 | self.update_params() 494 | self.update_param_texts() 495 | except TypeError: 496 | self.text_boxes["height_box"].\ 497 | set_val(str(self.shooting_cond["height"])) 498 | 499 | def horizontal_overwrap_update(self, event): 500 | """! 501 | Called when content of "Horizontal overwrap" is submitted 502 | @param event Content of TextBox 503 | """ 504 | # Update parameter if input is digit and valid value 505 | try: 506 | if 0 < float(event) < 1.0: 507 | self.coverage_params["horizontal_overwrap"].data = float(event) 508 | else: 509 | self.text_boxes["horizontal_overwrap_box"].\ 510 | set_val(str(self.coverage_params["horizontal_overwrap"].data)) 511 | except TypeError: 512 | self.text_boxes["horizontal_overwrap_box"].\ 513 | set_val(str(self.coverage_params["horizontal_overwrap"].data)) 514 | 515 | def vertical_overwrap_update(self, event): 516 | """! 517 | Called when content of "Vertical overwrap" is submitted 518 | @param event Content of TextBox 519 | """ 520 | # Update parameter if input is digit and valid value 521 | try: 522 | if 0 < float(event) < 1.0: 523 | self.coverage_params["vertical_overwrap"].data = float(event) 524 | else: 525 | self.text_boxes["vertical_overwrap_box"].\ 526 | set_val(str(self.coverage_params["vertical_overwrap"].data)) 527 | except TypeError: 528 | self.text_boxes["vertical_overwrap_box"].\ 529 | set_val(str(self.coverage_params["vertical_overwrap"].data)) 530 | 531 | 532 | def init_node(): 533 | """! 534 | Initialize a node 535 | """ 536 | rospy.init_node('specify_node', anonymous=True) 537 | 538 | # Call PolygonBuilder's constructor 539 | PolygonBuilder() 540 | 541 | 542 | if __name__ == '__main__': 543 | init_node() 544 | -------------------------------------------------------------------------------- /src/torres_etal_2016.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file torres_etal_2016.cpp 3 | * @brief Coverage path planner based on M. Torres et al, 2016 4 | * @author Takaki Ueno 5 | */ 6 | 7 | /* 8 | * Copyright (c) 2017 Takaki Ueno 9 | * Released under the MIT license 10 | */ 11 | 12 | // header 13 | #include 14 | 15 | /** 16 | * @brief Generate vector of polygon from PointVector 17 | * @param polygon 18 | * @return std::vector Vector of subpolygons assumed to be passed to ROS msg 19 | */ 20 | std::vector generatePolygonVector(const PointVector& polygon) 21 | { 22 | // convert PointVector (a.k.a. std::vector) to geometry_msgs::Polygon 23 | // so that polygon is visualized on the window 24 | geometry_msgs::Polygon poly; 25 | for (const auto& vertex : polygon) 26 | { 27 | geometry_msgs::Point32 p; 28 | p.x = vertex.x; 29 | p.y = vertex.y; 30 | poly.points.push_back(p); 31 | } 32 | 33 | std::vector polygons = { poly }; 34 | 35 | return polygons; 36 | } 37 | 38 | /** 39 | * @brief Generate vector of polygon from std::vector 40 | * @param subPolygons 41 | * @return std::vector Vector of subpolygons assumed to be passed to ROS msg 42 | */ 43 | std::vector generatePolygonVector(const std::vector& subPolygons) 44 | { 45 | std::vector subPolygonsRet; 46 | 47 | // convert PointVector (a.k.a. std::vector) to geometry_msgs::Polygon 48 | // so that polygon is visualized on the window 49 | for (const auto& subPolygon : subPolygons) 50 | { 51 | geometry_msgs::Polygon poly; 52 | for (const auto& vertex : subPolygon) 53 | { 54 | geometry_msgs::Point32 pt; 55 | pt.x = vertex.x; 56 | pt.y = vertex.y; 57 | poly.points.push_back(pt); 58 | } 59 | subPolygonsRet.push_back(poly); 60 | } 61 | 62 | return subPolygonsRet; 63 | } 64 | 65 | /** 66 | * @brief Plans coverage path 67 | * @param req Contains values neccesary to plan a path 68 | * @param res Contains resulting path 69 | * @return bool 70 | * @details For details of this service, cf. srv/Torres16.srv 71 | */ 72 | bool plan(cpp_uav::Torres16::Request& req, cpp_uav::Torres16::Response& res) 73 | { 74 | // see torres et al. 2016 for the flow of this algorithm 75 | 76 | // polygon from request and path for response 77 | PointVector polygon, candidatePath; 78 | // start point of coverage path 79 | geometry_msgs::Point start; 80 | // parameters of coverage path 81 | std_msgs::Float64 footprintLength, footprintWidth, horizontalOverwrap, verticalOverwrap; 82 | 83 | polygon = req.polygon; 84 | start = req.start; 85 | 86 | footprintLength = req.footprint_length; 87 | footprintWidth = req.footprint_width; 88 | horizontalOverwrap = req.horizontal_overwrap; 89 | verticalOverwrap = req.vertical_overwrap; 90 | 91 | // isOptimal is true if computed path does not have intersection with polygon 92 | bool isOptimal = computeConvexCoverage(polygon, footprintWidth.data, horizontalOverwrap.data, candidatePath); 93 | 94 | if (isOptimal == true) 95 | { 96 | // fill "subpolygon" field of response so that polygon is visualized 97 | res.subpolygons = generatePolygonVector(polygon); 98 | 99 | // set optimal alternative as optimal path 100 | // see torres et al. 2016 for Optimal Alternative 101 | res.path = identifyOptimalAlternative(polygon, candidatePath, start); 102 | } 103 | else 104 | { 105 | std::vector subPolygons = decomposePolygon(polygon); 106 | 107 | // sum of length of all coverage path 108 | double pathLengthSum = 0; 109 | 110 | int i = 1; 111 | // compute length of coverage path of each subpolygon 112 | for (const auto& polygon : subPolygons) 113 | { 114 | PointVector partialPath; 115 | computeConvexCoverage(polygon, footprintWidth.data, horizontalOverwrap.data, partialPath); 116 | pathLengthSum += calculatePathLength(partialPath); 117 | } 118 | 119 | // existsSecondOptimalPath is true if there is at least one coverage that has no intersection with polygon 120 | // second optimal path is the path that has second shortest sweep direction and no intersection with polygon 121 | PointVector secondOptimalPath; 122 | bool existsSecondOptimalPath = 123 | findSecondOptimalPath(polygon, footprintWidth.data, horizontalOverwrap.data, candidatePath); 124 | 125 | if (existsSecondOptimalPath == true) 126 | { 127 | // compute optimal alternative for second optimal path 128 | secondOptimalPath = identifyOptimalAlternative(polygon, candidatePath, start); 129 | 130 | // if the length of second optimal path is shorter than the sum of coverage path of subpolygons, 131 | // set second optimal path as the path 132 | if (pathLengthSum > calculatePathLength(secondOptimalPath)) 133 | { 134 | // fill "subpolygon" field of response so that polygon is visualized 135 | res.subpolygons = generatePolygonVector(polygon); 136 | 137 | res.path = secondOptimalPath; 138 | 139 | return true; 140 | 141 | } 142 | // returns second optimal path when shortest path is not optimal and polygon cannot be decomposed 143 | else if(subPolygons.size() == 1) 144 | { 145 | res.subpolygons = generatePolygonVector(polygon); 146 | 147 | res.path = secondOptimalPath; 148 | 149 | return true; 150 | } 151 | } 152 | else if (subPolygons.size() < 2) 153 | { 154 | // if number of subpolygon is smaller than 2, 155 | // it means no valid path can be computed 156 | ROS_ERROR("Unable to generate path."); 157 | return true; 158 | } 159 | 160 | // fill "subpolygon" field of response so that polygon is visualized 161 | res.subpolygons = generatePolygonVector(subPolygons); 162 | 163 | // compute coverage path of subpolygons 164 | PointVector multipleCoveragePath = 165 | computeMultiplePolygonCoverage(subPolygons, footprintWidth.data, horizontalOverwrap.data); 166 | 167 | res.path = multipleCoveragePath; 168 | } 169 | 170 | return true; 171 | } 172 | 173 | int main(int argc, char** argv) 174 | { 175 | ros::init(argc, argv, "torres_etal_2016"); 176 | ros::NodeHandle nh; 177 | 178 | ros::ServiceServer planner = nh.advertiseService("cpp_torres16", plan); 179 | ROS_INFO("Ready to plan."); 180 | 181 | ros::spin(); 182 | 183 | return 0; 184 | } 185 | -------------------------------------------------------------------------------- /srv/Torres16.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point[] polygon 2 | geometry_msgs/Point start 3 | std_msgs/Float64 footprint_length 4 | std_msgs/Float64 footprint_width 5 | std_msgs/Float64 horizontal_overwrap 6 | std_msgs/Float64 vertical_overwrap 7 | --- 8 | geometry_msgs/Point[] path 9 | geometry_msgs/Polygon[] subpolygons -------------------------------------------------------------------------------- /test/test_cgutil.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file test_cgutil.cpp 3 | * @brief Test program for cgutil.hpp 4 | * @author Takaki Ueno 5 | */ 6 | 7 | /* 8 | * Copyright (c) 2017 Takaki Ueno 9 | * Released under the MIT license 10 | */ 11 | 12 | #include 13 | 14 | // gtest 15 | #include 16 | 17 | // c++ libraries 18 | #include 19 | #include 20 | #include 21 | 22 | // geometry_msgs 23 | #include 24 | 25 | TEST(CGUtilTest, IntersectTest1) 26 | { 27 | geometry_msgs::Point p1, p2, p3, p4, p5, p6, p7, p8; 28 | std::array e1, e2, e3, e4; 29 | std::vector > segments; 30 | 31 | p1.x = 0.0; 32 | p1.y = 0.0; 33 | p2.x = 5.0; 34 | p2.y = 1.0; 35 | p3.x = 2.0; 36 | p3.y = 5.0; 37 | p4.x = 6.0; 38 | p4.y = 3.0; 39 | p5.x = 7.0; 40 | p5.y = 1.0; 41 | p6.x = 8.0; 42 | p6.y = 5.0; 43 | p7.x = 3.0; 44 | p7.y = 3.0; 45 | p8.x = 5.0; 46 | p8.y = 5.0; 47 | 48 | e1.at(0) = p1; 49 | e1.at(1) = p2; 50 | e2.at(0) = p3; 51 | e2.at(1) = p4; 52 | e3.at(0) = p5; 53 | e3.at(1) = p6; 54 | e4.at(0) = p7; 55 | e4.at(1) = p8; 56 | 57 | segments.push_back(e1); 58 | segments.push_back(e2); 59 | segments.push_back(e3); 60 | segments.push_back(e4); 61 | 62 | std::vector, 2> > intersecting_segments = intersect(segments); 63 | EXPECT_EQ(1, intersecting_segments.size()); 64 | EXPECT_DOUBLE_EQ(p3.x, intersecting_segments.at(0).at(0).at(0).x); 65 | EXPECT_DOUBLE_EQ(p3.y, intersecting_segments.at(0).at(0).at(0).y); 66 | EXPECT_DOUBLE_EQ(p4.x, intersecting_segments.at(0).at(0).at(1).x); 67 | EXPECT_DOUBLE_EQ(p4.y, intersecting_segments.at(0).at(0).at(1).y); 68 | EXPECT_DOUBLE_EQ(p7.x, intersecting_segments.at(0).at(1).at(0).x); 69 | EXPECT_DOUBLE_EQ(p7.y, intersecting_segments.at(0).at(1).at(0).y); 70 | EXPECT_DOUBLE_EQ(p8.x, intersecting_segments.at(0).at(1).at(1).x); 71 | EXPECT_DOUBLE_EQ(p8.y, intersecting_segments.at(0).at(1).at(1).y); 72 | } 73 | 74 | TEST(CGUtilTest, IntersectTest2) 75 | { 76 | geometry_msgs::Point p1, p2, p3, p4; 77 | std::array e1, e2; 78 | 79 | // TestCase1 80 | // p1: (0, 0) 81 | // p2: (3, 2) 82 | // p3: (2, 0) 83 | // p4: (0, 2) 84 | // Expected value: true 85 | p1.x = 0.0; 86 | p1.y = 0.0; 87 | p2.x = 3.0; 88 | p2.y = 2.0; 89 | p3.x = 2.0; 90 | p3.y = 0.0; 91 | p4.x = 0.0; 92 | p4.y = 2.0; 93 | 94 | e1.at(0) = p1; 95 | e1.at(1) = p2; 96 | e2.at(0) = p3; 97 | e2.at(1) = p4; 98 | 99 | EXPECT_TRUE(intersect(e1, e2)); 100 | 101 | // TestCase2 102 | // p1: (0, 0) 103 | // p2: (4, 2) 104 | // p3: (2, 1) 105 | // p4: (0, 2) 106 | // Expected value: true 107 | p1.x = 0.0; 108 | p1.y = 0.0; 109 | p2.x = 4.0; 110 | p2.y = 2.0; 111 | p3.x = 2.0; 112 | p3.y = 1.0; 113 | p4.x = 0.0; 114 | p4.y = 2.0; 115 | 116 | e1.at(0) = p1; 117 | e1.at(1) = p2; 118 | e2.at(0) = p3; 119 | e2.at(1) = p4; 120 | 121 | EXPECT_TRUE(intersect(e1, e2)); 122 | 123 | // TestCase3 124 | // p1: (0, 0) 125 | // p2: (4, 2) 126 | // p3: (1, 1) 127 | // p4: (0, 2) 128 | // Expected value: false 129 | p1.x = 0.0; 130 | p1.y = 0.0; 131 | p2.x = 4.0; 132 | p2.y = 2.0; 133 | p3.x = 1.0; 134 | p3.y = 1.0; 135 | p4.x = 0.0; 136 | p4.y = 2.0; 137 | 138 | e1.at(0) = p1; 139 | e1.at(1) = p2; 140 | e2.at(0) = p3; 141 | e2.at(1) = p4; 142 | 143 | EXPECT_FALSE(intersect(e1, e2)); 144 | } 145 | 146 | TEST(CGUtilTest, InBetweenTest) 147 | { 148 | geometry_msgs::Point p1, p2, p3; 149 | 150 | // TestCase1 151 | // p1: (0, 0) 152 | // p2: (2, 2) 153 | // p3: (1, 1) 154 | // Expected value: true 155 | p1.x = 0.0; 156 | p1.y = 0.0; 157 | p2.x = 2.0; 158 | p2.y = 2.0; 159 | p3.x = 1.0; 160 | p3.y = 1.0; 161 | EXPECT_TRUE(inBetween(p1, p2, p3)); 162 | 163 | // TestCase2 164 | // p1: (0, 0) 165 | // p2: (2, 2) 166 | // p3: (1, 0) 167 | // Expected value: false 168 | p1.x = 0.0; 169 | p1.y = 0.0; 170 | p2.x = 2.0; 171 | p2.y = 2.0; 172 | p3.x = 1.0; 173 | p3.y = 0.0; 174 | EXPECT_FALSE(inBetween(p1, p2, p3)); 175 | 176 | // TestCase3 177 | // p1: (0, 0) 178 | // p2: (0, 0) 179 | // p3: (0, 0) 180 | // Expected value: true 181 | p1.x = 0.0; 182 | p1.y = 0.0; 183 | p2.x = 0.0; 184 | p2.y = 0.0; 185 | p3.x = 0.0; 186 | p3.y = 0.0; 187 | EXPECT_TRUE(inBetween(p1, p2, p3)); 188 | } 189 | 190 | /** 191 | * @brief Test for distance() 192 | */ 193 | TEST(CGUtilTest, CalcDistanceTest) 194 | { 195 | // TestCase1 196 | // coordinate of edge's start point: (0.0, 0.0) 197 | // coordinate of edge's end point: (1.0, 0.0) 198 | // coordinate of opposite vertex: (0.5, 0.5) 199 | // expected distance: 0.5 200 | std::array edge; 201 | geometry_msgs::Point e1, e2, vertex; 202 | 203 | e1.x = 0.0; 204 | e1.y = 0.0; 205 | e2.x = 1.0; 206 | e2.y = 0.0; 207 | edge.front() = e1; 208 | edge.back() = e2; 209 | 210 | vertex.x = 0.5; 211 | vertex.y = 0.5; 212 | 213 | EXPECT_NEAR(0.5, distance(edge, vertex), 1.0e-5); 214 | 215 | // TestCase2 216 | // coordinate of edge's start point: (0.0, 0.0) 217 | // coordinate of edge's end point: (0.0, sqrt(3.0)/2) 218 | // coordinate of opposite vertex: (0.0, 0.5) 219 | // expected distance: 0.5 220 | e1.x = 0.0; 221 | e1.y = 0.0; 222 | e2.x = 0.866; 223 | e2.y = 0.0; 224 | edge.front() = e1; 225 | edge.back() = e2; 226 | 227 | vertex.x = 0.0; 228 | vertex.y = 0.5; 229 | 230 | EXPECT_NEAR(0.5, distance(edge, vertex), 1.0e-5); 231 | 232 | // TestCase3 233 | // coordinate of edge's start point: (0.0, 0.0) 234 | // coordinate of edge's end point: (1.0, 0.0) 235 | // coordinate of opposite vertex: (-1.0, 1.0) 236 | // expected distance: 1.0 237 | e1.x = 0.0; 238 | e1.y = 0.0; 239 | e2.x = 1.0; 240 | e2.y = 0.0; 241 | edge.front() = e1; 242 | edge.back() = e2; 243 | 244 | vertex.x = -1.0; 245 | vertex.y = 1.0; 246 | 247 | EXPECT_NEAR(1.0, distance(edge, vertex), 1.0e-5); 248 | 249 | // TestCase4 250 | // coordinate of edge's start point: (0.0, 0.0) 251 | // coordinate of edge's end point: (1.0, 0.0) 252 | // coordinate of opposite vertex: (1.5, 0.5) 253 | // expected distance: 0.5 254 | e1.x = 0.0; 255 | e1.y = 0.0; 256 | e2.x = 1.0; 257 | e2.y = 0.0; 258 | edge.front() = e1; 259 | edge.back() = e2; 260 | 261 | vertex.x = 1.5; 262 | vertex.y = 0.5; 263 | 264 | EXPECT_NEAR(0.5, distance(edge, vertex), 1.0e-5); 265 | 266 | // TestCase5 267 | // coordinate of edge's start point: (0.0, 0.0) 268 | // coordinate of edge's end point: (1.0, -1/2) 269 | // coordinate of opposite vertex: (2/3, 1/3) 270 | // expected distance: 4*sqrt(5)/15 271 | e1.x = 0.0; 272 | e1.y = 0.0; 273 | e2.x = 1.0; 274 | e2.y = -0.5; 275 | edge.front() = e1; 276 | edge.back() = e2; 277 | 278 | vertex.x = 2.0 / 3.0; 279 | vertex.y = 1.0 / 3.0; 280 | 281 | EXPECT_NEAR(0.596284794, distance(edge, vertex), 1.0e-5); 282 | 283 | // TestCase6 284 | // coordinate of edge's start point: (1.5, 1.0) 285 | // coordinate of edge's end point: (0.0, 2.5) 286 | // coordinate of opposite vertex: (0.5, 1.5) 287 | // expected distance: sqrt(2)/4 288 | e1.x = 1.5; 289 | e1.y = 1.0; 290 | e2.x = 0; 291 | e2.y = 2.5; 292 | edge.front() = e1; 293 | edge.back() = e2; 294 | 295 | vertex.x = 0.5; 296 | vertex.y = 1.5; 297 | 298 | EXPECT_NEAR(0.3535533906, distance(edge, vertex), 1.0e-5); 299 | 300 | // TestCase7 301 | // coordinate of edge's start point: (6.0, 1.0) 302 | // coordinate of edge's end point: (0.5, 1.0) 303 | // coordinate of opposite vertex: (1.5, 2.5) 304 | // expected distance: 3/2 305 | e1.x = 6.0; 306 | e1.y = 1.0; 307 | e2.x = 0.5; 308 | e2.y = 1.0; 309 | edge.front() = e1; 310 | edge.back() = e2; 311 | 312 | vertex.x = 1.5; 313 | vertex.y = 2.5; 314 | 315 | EXPECT_NEAR(1.5, distance(edge, vertex), 1.0e-5); 316 | } 317 | 318 | /** 319 | * @brief Test for vertexAngle() 320 | */ 321 | TEST(CGUtilTest, CalcVertexAngleTest) 322 | { 323 | geometry_msgs::Point p1, p2, p3; 324 | 325 | // TestCase1 326 | // p1: (0, 0) 327 | // p2: (1, 0) 328 | // p3: (1, 1) 329 | // Expected angle[rad]: pi/4 330 | p1.x = 0.0; 331 | p1.y = 0.0; 332 | p2.x = 1.0; 333 | p2.y = 0.0; 334 | p3.x = 1.0; 335 | p3.y = 1.0; 336 | EXPECT_DOUBLE_EQ(M_PI_4, vertexAngle(p1, p2, p3)); 337 | 338 | // TestCase2 339 | // p1: (0, 0) 340 | // p2: (1, 0) 341 | // p3: (0, 0) 342 | // Expected angle[rad]: 0 343 | p1.x = 0.0; 344 | p1.y = 0.0; 345 | p2.x = 1.0; 346 | p2.y = 0.0; 347 | p3.x = 0.0; 348 | p3.y = 0.0; 349 | EXPECT_DOUBLE_EQ(0.0, vertexAngle(p1, p2, p3)); 350 | 351 | // TestCase3 352 | // p1: (0, 0) 353 | // p2: (1, 0) 354 | // p3: (0, 1) 355 | // Expected angle[rad]: pi/2 356 | p1.x = 0.0; 357 | p1.y = 0.0; 358 | p2.x = 1.0; 359 | p2.y = 0.0; 360 | p3.x = 0.0; 361 | p3.y = 1.0; 362 | EXPECT_DOUBLE_EQ(M_PI_2, vertexAngle(p1, p2, p3)); 363 | 364 | // TestCase4 365 | // p1: (0, 0) 366 | // p2: (1, 0) 367 | // p3: (-1, 1) 368 | // Expected angle[rad]: 3*pi/4 369 | p1.x = 0.0; 370 | p1.y = 0.0; 371 | p2.x = 1.0; 372 | p2.y = 0.0; 373 | p3.x = -1.0; 374 | p3.y = 1.0; 375 | EXPECT_DOUBLE_EQ(3 * M_PI_4, vertexAngle(p1, p2, p3)); 376 | 377 | // TestCase5 378 | // p1: (0, 0) 379 | // p2: (-1, 0) 380 | // p3: (0.5, sqrt(3)/2) 381 | // Expected angle[rad]: 2*pi/3 382 | p1.x = 0.0; 383 | p1.y = 0.0; 384 | p2.x = -1.0; 385 | p2.y = 0.0; 386 | p3.x = 0.5; 387 | p3.y = std::sqrt(3.0) / 2.0; 388 | EXPECT_DOUBLE_EQ(2 * M_PI / 3.0, vertexAngle(p1, p2, p3)); 389 | 390 | // TestCase6 391 | // p1: (0, 0) 392 | // p2: (1, 0) 393 | // p3: (-sqrt(3)/2, -0.5) 394 | // Expected angle[rad]: 2*pi/3 395 | p1.x = 0.0; 396 | p1.y = 0.0; 397 | p2.x = 1.0; 398 | p2.y = 0.0; 399 | p3.x = -std::sqrt(3.0) / 2.0; 400 | p3.y = -0.5; 401 | EXPECT_DOUBLE_EQ(5 * M_PI / 6.0, vertexAngle(p1, p2, p3)); 402 | 403 | // TestCase7 404 | // p1: (0, 0) 405 | // p2: (0, 0) 406 | // p3: (0, 0) 407 | // Expected angle[rad]: 0.0 408 | p1.x = 0.0; 409 | p1.y = 0.0; 410 | p2.x = 0.0; 411 | p2.y = 0.0; 412 | p3.x = 0.0; 413 | p3.y = 0.0; 414 | EXPECT_DOUBLE_EQ(0.0, vertexAngle(p1, p2, p3)); 415 | } 416 | 417 | /** 418 | * @brief Test for horizontalAngle() 419 | */ 420 | TEST(CGUtilTest, CalcHorizontalAngleTest) 421 | { 422 | geometry_msgs::Point p1, p2; 423 | 424 | // TestCase1 425 | // p1: (0, 0) 426 | // p2: (1, 0) 427 | // Expected angle[rad]: 0 428 | p1.x = 0.0; 429 | p1.y = 0.0; 430 | p2.x = 1.0; 431 | p2.y = 0.0; 432 | EXPECT_DOUBLE_EQ(0, horizontalAngle(p1, p2)); 433 | 434 | // TestCase2 435 | // p1: (0, 0) 436 | // p2: (1, 1) 437 | // Expected angle[rad]: pi/4 438 | p1.x = 0.0; 439 | p1.y = 0.0; 440 | p2.x = 1.0; 441 | p2.y = 1.0; 442 | EXPECT_DOUBLE_EQ(M_PI_4, horizontalAngle(p1, p2)); 443 | 444 | // TestCase3 445 | // p1: (0, 0) 446 | // p2: (-0.5, sqrt(3)/2) 447 | // Expected angle[rad]: 3*pi/4 448 | p1.x = 0.0; 449 | p1.y = 0.0; 450 | p2.x = -0.5; 451 | p2.y = std::sqrt(3) / 2.0; 452 | EXPECT_DOUBLE_EQ(2 * M_PI / 3, horizontalAngle(p1, p2)); 453 | 454 | // TestCase4 455 | // p1: (0, 0) 456 | // p2: (0, -1) 457 | // Expected angle[rad]: pi/2 458 | p1.x = 0.0; 459 | p1.y = 0.0; 460 | p2.x = 0.0; 461 | p2.y = -1.0; 462 | EXPECT_DOUBLE_EQ(M_PI_2, horizontalAngle(p1, p2)); 463 | 464 | // TestCase5 465 | // p1: (0, 0) 466 | // p2: (0, 0) 467 | // Expected angle[rad]: 0.0 468 | p1.x = 0.0; 469 | p1.y = 0.0; 470 | p2.x = 0.0; 471 | p2.y = 0.0; 472 | EXPECT_DOUBLE_EQ(0.0, horizontalAngle(p1, p2)); 473 | } 474 | 475 | /** 476 | * @brief Test for signedArea() 477 | */ 478 | TEST(CGUtilTest, CalcsignedAreaTest) 479 | { 480 | geometry_msgs::Point p1, p2, p3; 481 | 482 | // TestCase1 483 | // p1: (0, 0) 484 | // p2: (1, 0) 485 | // p3: (0, 1) 486 | // Expected value: 1 487 | p1.x = 0.0; 488 | p1.y = 0.0; 489 | p2.x = 1.0; 490 | p2.y = 0.0; 491 | p3.x = 0.0; 492 | p3.y = 1.0; 493 | EXPECT_DOUBLE_EQ(1.0, signedArea(p1, p2, p3)); 494 | 495 | // TestCase2 496 | // p1: (0, 0) 497 | // p2: (-1, 0) 498 | // p3: (0, 1) 499 | // Expected value: -1 500 | p1.x = 0.0; 501 | p1.y = 0.0; 502 | p2.x = -1.0; 503 | p2.y = 0.0; 504 | p3.x = 0.0; 505 | p3.y = 1.0; 506 | EXPECT_DOUBLE_EQ(-1.0, signedArea(p1, p2, p3)); 507 | 508 | // TestCase3 509 | // p1: (-1, 0) 510 | // p2: (1, 1) 511 | // p3: (2, -1) 512 | // Expected value: -5 513 | p1.x = -1.0; 514 | p1.y = 0.0; 515 | p2.x = 1.0; 516 | p2.y = 1.0; 517 | p3.x = 2.0; 518 | p3.y = -1.0; 519 | EXPECT_DOUBLE_EQ(-5.0, signedArea(p1, p2, p3)); 520 | 521 | // TestCase3 522 | // p1: (-1, 0) 523 | // p2: (1, 1) 524 | // p3: (1, 1) 525 | // Expected value: 0 526 | p1.x = -1.0; 527 | p1.y = 0.0; 528 | p2.x = 1.0; 529 | p2.y = 1.0; 530 | p3.x = 1.0; 531 | p3.y = 1.0; 532 | EXPECT_DOUBLE_EQ(0.0, signedArea(p1, p2, p3)); 533 | } 534 | 535 | /** 536 | * @brief Test for grahamScan() 537 | */ 538 | TEST(CGUtilTest, GrahamScanTest) 539 | { 540 | geometry_msgs::Point p1, p2, p3, p4, p5, p6, p7; 541 | std::vector points, convex_hull; 542 | 543 | // TestCase1 544 | // p1: (0, 0) 545 | // p2: (4, 1) 546 | // p3: (6, 4) 547 | // p4: (3, 5) 548 | // p5: (0, 3) 549 | // Expected points in convex hull: (p1, p2, p3, p4, p5) 550 | p1.x = 0.0; 551 | p1.y = 0.0; 552 | p2.x = 4.0; 553 | p2.y = 1.0; 554 | p3.x = 6.0; 555 | p3.y = 4.0; 556 | p4.x = 3.0; 557 | p4.y = 5.0; 558 | p5.x = 0.0; 559 | p5.y = 3.0; 560 | 561 | points.push_back(p1); 562 | points.push_back(p2); 563 | points.push_back(p3); 564 | points.push_back(p4); 565 | points.push_back(p5); 566 | 567 | convex_hull = grahamScan(points); 568 | 569 | EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x); 570 | EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y); 571 | EXPECT_DOUBLE_EQ(p2.x, convex_hull.at(1).x); 572 | EXPECT_DOUBLE_EQ(p2.y, convex_hull.at(1).y); 573 | EXPECT_DOUBLE_EQ(p3.x, convex_hull.at(2).x); 574 | EXPECT_DOUBLE_EQ(p3.y, convex_hull.at(2).y); 575 | EXPECT_DOUBLE_EQ(p4.x, convex_hull.at(3).x); 576 | EXPECT_DOUBLE_EQ(p4.y, convex_hull.at(3).y); 577 | EXPECT_DOUBLE_EQ(p5.x, convex_hull.at(4).x); 578 | EXPECT_DOUBLE_EQ(p5.y, convex_hull.at(4).y); 579 | 580 | points.clear(); 581 | 582 | // TestCase2 583 | // p1: (0, 0) 584 | // p2: (0, 3) 585 | // p3: (3, 3) 586 | // p4: (5, 4) 587 | // p5: (3, 0) 588 | // p6: (2, 5) 589 | // Expected points in convex hull: (p1, p5, p4, p6, p2) 590 | p1.x = 0.0; 591 | p1.y = 0.0; 592 | p2.x = 0.0; 593 | p2.y = 3.0; 594 | p3.x = 3.0; 595 | p3.y = 3.0; 596 | p4.x = 5.0; 597 | p4.y = 4.0; 598 | p5.x = 3.0; 599 | p5.y = 0.0; 600 | p6.x = 2.0; 601 | p6.y = 5.0; 602 | 603 | points.push_back(p1); 604 | points.push_back(p2); 605 | points.push_back(p3); 606 | points.push_back(p4); 607 | points.push_back(p5); 608 | points.push_back(p6); 609 | 610 | convex_hull = grahamScan(points); 611 | 612 | EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x); 613 | EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y); 614 | EXPECT_DOUBLE_EQ(p5.x, convex_hull.at(1).x); 615 | EXPECT_DOUBLE_EQ(p5.y, convex_hull.at(1).y); 616 | EXPECT_DOUBLE_EQ(p4.x, convex_hull.at(2).x); 617 | EXPECT_DOUBLE_EQ(p4.y, convex_hull.at(2).y); 618 | EXPECT_DOUBLE_EQ(p6.x, convex_hull.at(3).x); 619 | EXPECT_DOUBLE_EQ(p6.y, convex_hull.at(3).y); 620 | EXPECT_DOUBLE_EQ(p2.x, convex_hull.at(4).x); 621 | EXPECT_DOUBLE_EQ(p2.y, convex_hull.at(4).y); 622 | 623 | points.clear(); 624 | 625 | // TestCase3 626 | // p1: (0, 0) 627 | // p2: (5, 1) 628 | // p3: (5, 4) 629 | // p4: (3, 2) 630 | // p5: (1, 3) 631 | // p6: (2, 5) 632 | // p7: (0, 4) 633 | // Expected points in convex hull: (p1, p2, p3, p6, p7) 634 | p1.x = 0.0; 635 | p1.y = 0.0; 636 | p2.x = 5.0; 637 | p2.y = 1.0; 638 | p3.x = 5.0; 639 | p3.y = 4.0; 640 | p4.x = 3.0; 641 | p4.y = 2.0; 642 | p5.x = 1.0; 643 | p5.y = 3.0; 644 | p6.x = 2.0; 645 | p6.y = 5.0; 646 | p7.x = 0.0; 647 | p7.y = 4.0; 648 | 649 | points.push_back(p1); 650 | points.push_back(p2); 651 | points.push_back(p3); 652 | points.push_back(p4); 653 | points.push_back(p5); 654 | points.push_back(p6); 655 | points.push_back(p7); 656 | 657 | convex_hull = grahamScan(points); 658 | 659 | EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x); 660 | EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y); 661 | EXPECT_DOUBLE_EQ(p2.x, convex_hull.at(1).x); 662 | EXPECT_DOUBLE_EQ(p2.y, convex_hull.at(1).y); 663 | EXPECT_DOUBLE_EQ(p3.x, convex_hull.at(2).x); 664 | EXPECT_DOUBLE_EQ(p3.y, convex_hull.at(2).y); 665 | EXPECT_DOUBLE_EQ(p6.x, convex_hull.at(3).x); 666 | EXPECT_DOUBLE_EQ(p6.y, convex_hull.at(3).y); 667 | EXPECT_DOUBLE_EQ(p7.x, convex_hull.at(4).x); 668 | EXPECT_DOUBLE_EQ(p7.y, convex_hull.at(4).y); 669 | 670 | points.clear(); 671 | 672 | // TestCase4 673 | // no points 674 | // Expected points in convex hull: () 675 | convex_hull = grahamScan(points); 676 | 677 | EXPECT_TRUE(convex_hull.empty()); 678 | 679 | points.clear(); 680 | 681 | // TestCase5 682 | // p1: (0, 0) 683 | // Expected points in convex hull: () 684 | p1.x = 0.0; 685 | p1.y = 0.0; 686 | 687 | points.push_back(p1); 688 | 689 | convex_hull = grahamScan(points); 690 | 691 | EXPECT_TRUE(convex_hull.empty()); 692 | 693 | points.clear(); 694 | 695 | // TestCase6 696 | // p1: (0, 0) 697 | // p2: (1, 0) 698 | // Expected points in convex hull: () 699 | p1.x = 0.0; 700 | p1.y = 0.0; 701 | p2.x = 1.0; 702 | p2.y = 0.0; 703 | 704 | points.push_back(p1); 705 | points.push_back(p2); 706 | 707 | convex_hull = grahamScan(points); 708 | 709 | EXPECT_TRUE(convex_hull.empty()); 710 | 711 | points.clear(); 712 | 713 | // TestCase7 714 | // p1: (0, 0) 715 | // p2: (1, 0) 716 | // p3: (1, 0) 717 | // Expected points in convex hull: () 718 | p1.x = 0.0; 719 | p1.y = 0.0; 720 | p2.x = 1.0; 721 | p2.y = 0.0; 722 | p3.x = 1.0; 723 | p3.y = 0.0; 724 | 725 | points.push_back(p1); 726 | points.push_back(p2); 727 | points.push_back(p3); 728 | 729 | convex_hull = grahamScan(points); 730 | 731 | EXPECT_TRUE(convex_hull.empty()); 732 | 733 | points.clear(); 734 | 735 | // TestCase8 736 | // p1: (0, 0) 737 | // p2: (1, 0) 738 | // p3: (1, 0) 739 | // p4: (1, 1) 740 | // Expected points in convex hull: (p1, p3, p4) 741 | p1.x = 0.0; 742 | p1.y = 0.0; 743 | p2.x = 1.0; 744 | p2.y = 0.0; 745 | p3.x = 1.0; 746 | p3.y = 0.0; 747 | p4.x = 1.0; 748 | p4.y = 1.0; 749 | 750 | points.push_back(p1); 751 | points.push_back(p2); 752 | points.push_back(p3); 753 | points.push_back(p4); 754 | 755 | convex_hull = grahamScan(points); 756 | 757 | EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x); 758 | EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y); 759 | EXPECT_DOUBLE_EQ(p3.x, convex_hull.at(1).x); 760 | EXPECT_DOUBLE_EQ(p3.y, convex_hull.at(1).y); 761 | EXPECT_DOUBLE_EQ(p4.x, convex_hull.at(2).x); 762 | EXPECT_DOUBLE_EQ(p4.y, convex_hull.at(2).y); 763 | 764 | points.clear(); 765 | } 766 | 767 | TEST(CGUtilTest, IsConvexTest1) 768 | { 769 | geometry_msgs::Point p1, p2, p3, p4, p5; 770 | std::vector points; 771 | 772 | // TestCase1 773 | // p1: (0, 0) 774 | // p2: (4, 1) 775 | // p3: (6, 4) 776 | // p4: (3, 5) 777 | // p5: (0, 3) 778 | // Expected value: True 779 | p1.x = 0.0; 780 | p1.y = 0.0; 781 | p2.x = 4.0; 782 | p2.y = 1.0; 783 | p3.x = 6.0; 784 | p3.y = 4.0; 785 | p4.x = 3.0; 786 | p4.y = 5.0; 787 | p5.x = 0.0; 788 | p5.y = 3.0; 789 | 790 | points.push_back(p1); 791 | points.push_back(p2); 792 | points.push_back(p3); 793 | points.push_back(p4); 794 | points.push_back(p5); 795 | 796 | EXPECT_TRUE(isConvex(points)); 797 | } 798 | 799 | TEST(CGUtilTest, IsConvexTest2) 800 | { 801 | geometry_msgs::Point p1, p2, p3, p4, p5; 802 | std::vector points; 803 | 804 | // TestCase2 805 | // p1: (0, 0) 806 | // p2: (1, 0) 807 | // p3: (1, 0) 808 | // p4: (1, 1) 809 | // Expected points in convex hull: (p1, p3, p4) 810 | p1.x = 0.0; 811 | p1.y = 0.0; 812 | p2.x = 1.0; 813 | p2.y = 0.0; 814 | p3.x = 1.0; 815 | p3.y = 0.0; 816 | p4.x = 1.0; 817 | p4.y = 1.0; 818 | 819 | points.push_back(p1); 820 | points.push_back(p2); 821 | points.push_back(p3); 822 | points.push_back(p4); 823 | 824 | EXPECT_FALSE(isConvex(points)); 825 | } 826 | 827 | int main(int argc, char** argv) 828 | { 829 | ::testing::InitGoogleTest(&argc, argv); 830 | return RUN_ALL_TESTS(); 831 | } 832 | -------------------------------------------------------------------------------- /test/test_torres_etal_2016.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file test_torres_etal_2016.cpp 3 | * @brief Test program for torres_etal_2016.hpp 4 | * @author Takaki Ueno 5 | */ 6 | 7 | /* 8 | * Copyright (c) 2017 Takaki Ueno 9 | * Released under the MIT license 10 | */ 11 | 12 | #include 13 | 14 | // gtest 15 | #include 16 | 17 | // c++ libraries 18 | #include 19 | #include 20 | 21 | // geometry_msgs 22 | #include 23 | 24 | /** 25 | * @brief Test for sweepDirection() 26 | */ 27 | TEST(Torres16Test, CalcSweepDirectionTest) 28 | { 29 | std::vector polygon; 30 | 31 | // Vertices of polygon 32 | geometry_msgs::Point p1, p2, p3, p4, p5; 33 | 34 | // Edges of polygon 35 | std::array e1, e2, e3, e4, e5; 36 | 37 | // TestCase1 38 | // coordinate of vertex1: (1.5, 2.5) 39 | // coordinate of vertex2: (0.5, 1.0) 40 | // coordinate of vertex3: (6.0, 1.0) 41 | // expected direction: edge2 -> vertex1 42 | p1.x = 1.5; 43 | p1.y = 2.5; 44 | p2.x = 0.5; 45 | p2.y = 1.0; 46 | p3.x = 6.0; 47 | p3.y = 1.0; 48 | polygon.push_back(p1); 49 | polygon.push_back(p2); 50 | polygon.push_back(p3); 51 | 52 | e1.at(0) = p1; 53 | e1.at(1) = p2; 54 | e2.at(0) = p2; 55 | e2.at(1) = p3; 56 | e3.at(0) = p3; 57 | e3.at(1) = p1; 58 | 59 | Direction dir = sweepDirection(polygon); 60 | 61 | EXPECT_DOUBLE_EQ(e2.front().x, dir.base_edge.front().x); 62 | EXPECT_DOUBLE_EQ(e2.front().y, dir.base_edge.front().y); 63 | EXPECT_DOUBLE_EQ(e2.back().x, dir.base_edge.back().x); 64 | EXPECT_DOUBLE_EQ(e2.back().y, dir.base_edge.back().y); 65 | EXPECT_DOUBLE_EQ(p1.x, dir.opposed_vertex.x); 66 | EXPECT_DOUBLE_EQ(p1.y, dir.opposed_vertex.y); 67 | 68 | polygon.clear(); 69 | 70 | // TestCase2 71 | // coordinate of vertex1: (2.0, 4.0) 72 | // coordinate of vertex2: (1.0, 1.0) 73 | // coordinate of vertex3: (5.0, 1.0) 74 | // coordinate of vertex4: (6.0, 3.0) 75 | // expected direction: edge2 -> vertex1 76 | p1.x = 2.0; 77 | p1.y = 4.0; 78 | p2.x = 1.0; 79 | p2.y = 1.0; 80 | p3.x = 5.0; 81 | p3.y = 1.0; 82 | p4.x = 6.0; 83 | p4.y = 3.0; 84 | polygon.push_back(p1); 85 | polygon.push_back(p2); 86 | polygon.push_back(p3); 87 | polygon.push_back(p4); 88 | 89 | e1.at(0) = p1; 90 | e1.at(1) = p2; 91 | e2.at(0) = p2; 92 | e2.at(1) = p3; 93 | e3.at(0) = p3; 94 | e3.at(1) = p4; 95 | e4.at(0) = p4; 96 | e4.at(1) = p1; 97 | 98 | dir = sweepDirection(polygon); 99 | 100 | EXPECT_DOUBLE_EQ(e2.front().x, dir.base_edge.front().x); 101 | EXPECT_DOUBLE_EQ(e2.front().y, dir.base_edge.front().y); 102 | EXPECT_DOUBLE_EQ(e2.back().x, dir.base_edge.back().x); 103 | EXPECT_DOUBLE_EQ(e2.back().y, dir.base_edge.back().y); 104 | EXPECT_DOUBLE_EQ(p1.x, dir.opposed_vertex.x); 105 | EXPECT_DOUBLE_EQ(p1.y, dir.opposed_vertex.y); 106 | 107 | polygon.clear(); 108 | 109 | // TestCase3 110 | // coordinate of vertex1: (1.0, 3.0) 111 | // coordinate of vertex2: (2.0, 1.0) 112 | // coordinate of vertex3: (5.0, 0.5) 113 | // coordinate of vertex4: (5.5, 2.5) 114 | // coordinate of vertex5: (3.0, 4.0) 115 | // expected direction: edge2 -> vertex5 116 | p1.x = 1.0; 117 | p1.y = 3.0; 118 | p2.x = 2.0; 119 | p2.y = 1.0; 120 | p3.x = 5.0; 121 | p3.y = 0.5; 122 | p4.x = 5.5; 123 | p4.y = 2.5; 124 | p5.x = 3.0; 125 | p5.y = 4.0; 126 | polygon.push_back(p1); 127 | polygon.push_back(p2); 128 | polygon.push_back(p3); 129 | polygon.push_back(p4); 130 | polygon.push_back(p5); 131 | 132 | e1.at(0) = p1; 133 | e1.at(1) = p2; 134 | e2.at(0) = p2; 135 | e2.at(1) = p3; 136 | e3.at(0) = p3; 137 | e3.at(1) = p4; 138 | e4.at(0) = p4; 139 | e4.at(1) = p5; 140 | e5.at(0) = p5; 141 | e5.at(1) = p1; 142 | 143 | dir = sweepDirection(polygon); 144 | 145 | EXPECT_DOUBLE_EQ(e4.front().x, dir.base_edge.front().x); 146 | EXPECT_DOUBLE_EQ(e4.front().y, dir.base_edge.front().y); 147 | EXPECT_DOUBLE_EQ(e4.back().x, dir.base_edge.back().x); 148 | EXPECT_DOUBLE_EQ(e4.back().y, dir.base_edge.back().y); 149 | EXPECT_DOUBLE_EQ(p2.x, dir.opposed_vertex.x); 150 | EXPECT_DOUBLE_EQ(p2.y, dir.opposed_vertex.y); 151 | 152 | polygon.clear(); 153 | } 154 | 155 | int main(int argc, char** argv) 156 | { 157 | ::testing::InitGoogleTest(&argc, argv); 158 | return RUN_ALL_TESTS(); 159 | } 160 | --------------------------------------------------------------------------------