├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── HISTORY ├── LICENSE ├── README.md ├── cmake └── Modules │ ├── FindCxxopts.cmake │ ├── FindEigen3.cmake │ ├── FindOgdf.cmake │ └── FindStacktrace.cmake ├── data ├── experiment │ ├── 2015.11.07_phoenixflight1_3tours.eps │ ├── 2015.11.07_phoenixflight1_3tours.png │ ├── 2015.11.07_phoenixflight1_alldata.json │ ├── 2015.11.07_phoenixflight1_gps.csv │ ├── 2015.11.07_phoenixflight1_gpsraw.csv │ ├── 2015.11.07_phoenixflight1_tour_notes.md │ ├── 2015.11.07_phoenixtour_notes.md │ ├── 2015.11.07_ptp_tour.csv │ ├── 2015.11.07_ptp_tour.gml │ ├── 2015.11.18_cpp_polygon.gml │ ├── 2015.11.18_cpp_tour.csv │ ├── 2015.11.18_phoenixflight2.png │ ├── 2015.11.18_phoenixflight2_gps.csv │ ├── phoenixflight1Data.m │ └── phoenixflight2.m ├── test │ ├── cpp │ │ ├── examplePolygon1.gml │ │ ├── isoscelesTrapezoid.gml │ │ ├── isoscelesTriangle.gml │ │ ├── nonConvexPolygon.gml │ │ ├── rectangle.gml │ │ ├── rightTriangle.gml │ │ ├── square.gml │ │ └── trapezoid.gml │ └── dtsp │ │ ├── square-36wp.gml │ │ ├── triangle-27wp.gml │ │ ├── triangle-8wp.dot │ │ └── triangle-8wp.gml └── trash │ ├── square_polygon.dlm │ ├── square_polygon.gml │ ├── testing.dot │ ├── testing.gml │ ├── triangle_polygon.dlm │ └── triangle_polygon.gml ├── examples ├── .gitignore ├── CMakeLists.txt ├── CMakeLists_example.txt ├── README.md ├── boost │ ├── directed_graph_test.cpp │ ├── dubins_graph_test.cpp │ ├── euclidean_tsp.cpp │ ├── read_graphviz.cpp │ ├── test.dot │ └── write_graphviz.cpp ├── ogdf │ ├── CMakeLists.txt │ ├── manual.cpp │ ├── manual_mod.cpp │ ├── random.cpp │ └── sierpinski.cpp ├── snap │ ├── CMakeLists.txt │ └── directed_graph1.cpp ├── solveDTSP_example.cpp └── solveDtspSimple_example.cpp ├── include ├── dpp │ ├── basic │ │ ├── Field.h │ │ ├── FieldTrack.h │ │ ├── FileIO.h │ │ ├── Logger.h │ │ ├── NodeMatrix.h │ │ ├── Path.h │ │ ├── Util.h │ │ ├── VehicleConfiguration.h │ │ └── basic.h │ ├── planalg │ │ ├── Algorithm.h │ │ ├── AlternatingDtsp.h │ │ ├── BoustrophedonCpp.h │ │ ├── NearestNeighborDtsp.h │ │ └── RandomizedDtsp.h │ └── planner │ │ ├── CoverageWaypointPlanner.h │ │ ├── DubinsSensorPathPlanner.h │ │ ├── DubinsVehiclePathPlanner.h │ │ ├── PathPlanner.h │ │ ├── PathPlannerException.h │ │ └── WaypointSequencePlanner.h ├── solveCpp.h ├── solveCppAsDtsp.h └── solveDtsp.h ├── lib ├── .gitignore ├── README.md └── stacktrace │ └── stacktrace.h ├── src ├── CMakeLists.txt ├── computeDubinsDistancesATSP.cpp ├── dpp │ ├── basic │ │ ├── Field.cpp │ │ ├── FieldTrack.cpp │ │ ├── FileIO.cpp │ │ ├── Logger.cpp │ │ ├── Path.cpp │ │ └── basic.cpp │ ├── planalg │ │ ├── Algorithm.cpp │ │ ├── AlternatingDtsp.cpp │ │ ├── BoustrophedonCpp.cpp │ │ ├── NearestNeighborDtsp.cpp │ │ └── RandomizedDtsp.cpp │ └── planner │ │ ├── CoverageWaypointPlanner.cpp │ │ ├── DubinsSensorPathPlanner.cpp │ │ ├── DubinsVehiclePathPlanner.cpp │ │ ├── PathPlanner.cpp │ │ └── WaypointSequencePlanner.cpp ├── mex │ ├── mexHelper.cpp │ ├── mexHelper.h │ ├── solveCppAsDtsp_interface.cpp │ ├── solveCpp_interface.cpp │ └── solveDtsp_interface.cpp ├── solveCpp.cpp ├── solveCppAsDtsp.cpp └── solveDtsp.cpp └── test ├── CMakeLists.txt ├── DubinsPathPlanner_test.h ├── basic ├── Field_test.cpp ├── FileIO_test.cpp ├── Logger_test.cpp ├── Path_test.cpp └── Util_test.cpp ├── planalg ├── AlternatingDtsp_test.cpp ├── NearestNeighborDtsp_test.cpp └── RandomizedDtsp_test.cpp └── planner ├── CoverageWaypointPlanner_test.cpp ├── DubinsSensorPathPlanner_test.cpp ├── DubinsVehiclePathPlanner_test.cpp └── WaypointSequencePlanner_test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Do not track files with the following extensions 2 | *.m~ 3 | .DS_Store 4 | ~*.tmp 5 | *~ 6 | ~$* 7 | *.swp 8 | *.swo 9 | build* 10 | cscope.out 11 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "matlab"] 2 | path = matlab 3 | url = git@github.com:dagoodma/dppl_matlab.git 4 | branch = master 5 | [submodule "test/gtest"] 6 | path = test/gtest 7 | url = https://github.com/google/googletest.git 8 | [submodule "lib/cxxopts"] 9 | path = lib/cxxopts 10 | url = https://github.com/jarro2783/cxxopts.git 11 | branch = master 12 | [submodule "lib/dubins-curves"] 13 | path = lib/dubins-curves 14 | url = git@github.com:dagoodma/Dubins-Curves.git 15 | branch = cpp 16 | [submodule "ogdf"] 17 | path = ogdf 18 | url = https://github.com/ogdf/ogdf.git 19 | branch = master 20 | [submodule "lib/ogdf"] 21 | path = lib/ogdf 22 | url = https://github.com/ogdf/ogdf.git 23 | branch = master 24 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists 2 | cmake_minimum_required(VERSION 3.1) 3 | project (dubinsAreaCoverage CXX) 4 | 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/Modules/") 6 | 7 | enable_testing() 8 | 9 | ################################ 10 | # OGDF 11 | ################################ 12 | # FIXME using hardcoded ogdf variables 13 | #find_package(Ogdf REQUIRED) 14 | #include_directories( ${OGDF_INCLUDE_DIRS} ) 15 | 16 | set(OGDF_SOURCE_DIR ${CMAKE_SOURCE_DIR}/lib/ogdf ) 17 | 18 | find_library(OGDF ogdf) 19 | find_library(COIN coin) 20 | 21 | 22 | # Look for include directories 23 | find_path(OGDF_INCLUDE_DIR ogdf/basic/basic.h coin/Coin_C_defines.h 24 | PATHS: ${OGDF_SOURCE_DIR}/include ) 25 | 26 | if (OGDF_INCLUDE_DIR STREQUAL "OGDF_INCLUDE_DIR-NOTFOUND") 27 | message(FATAL_ERROR "OGDF include directory could not be found!") 28 | endif() 29 | 30 | include_directories( ${OGDF_INCLUDE_DIR} ) 31 | 32 | # Look for libraries 33 | find_path(OGDF_LIBRARY_DIR libCOIN.a libOGDF.a 34 | PATHS: ${OGDF_SOURCE_DIR}/_debug ${OGDF_SOURCE_DIR}/_release ) 35 | 36 | if (OGDF_LIBRARY_DIR STREQUAL "OGDF_LIBRARY_DIR-NOTFOUND") 37 | message(FATAL_ERROR "OGDF libraries could not be found!") 38 | endif() 39 | 40 | # Add libraries to link directories 41 | link_directories( ${OGDF_SOURCE_DIR}/_debug ) 42 | if ( CMAKE_BUILD_TYPE MATCHES Debug ) 43 | link_directories( ${OGDF_SOURCE_DIR}/_debug ) 44 | else() 45 | link_directories( ${OGDF_SOURCE_DIR}/_release ) 46 | endif() 47 | 48 | set( OGDF_LIBRARIES OGDF COIN ) 49 | 50 | ################################ 51 | # LKH Binary 52 | ################################ 53 | find_program(LKH_EXECUTABLE LKH . PATHS: lib/LKH lib/LKH-2.0.7 ) 54 | message("-- Found LKH binary: " ${LKH_EXECUTABLE}) 55 | 56 | if ( LKH_EXECUTABLE STREQUAL "LKH_EXECUTABLE-NOTFOUND" ) 57 | message(FATAL_ERROR "LKH executable could not found!") 58 | else() 59 | set( LKH_EXECUTABLE_FLAG "-DLKH_EXECUTABLE='\"${LKH_EXECUTABLE}\"'") 60 | set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${LKH_EXECUTABLE_FLAG}" ) 61 | endif() 62 | 63 | ################################ 64 | # Dubins-Curves Libraries 65 | ################################ 66 | 67 | link_directories( ${CMAKE_SOURCE_DIR}/lib/dubins-curves ) 68 | include_directories( ${CMAKE_SOURCE_DIR}/lib/dubins-curves/include ) 69 | 70 | ################################ 71 | # Eigen and others 72 | ################################ 73 | # Header-only libraries are include only 74 | find_package(Eigen3 REQUIRED) 75 | include_directories( ${EIGEN3_INCLUDE_DIR} ) 76 | 77 | find_package(Cxxopts REQUIRED) 78 | include_directories( ${CXXOPTS_INCLUDE_DIR} ) 79 | 80 | find_package(Stacktrace REQUIRED) 81 | include_directories( ${STACKTRACE_INCLUDE_DIR} ) 82 | 83 | ################################ 84 | # Dubins Path Planner 85 | ################################ 86 | # DPP library includes 87 | include_directories( ${CMAKE_SOURCE_DIR}/include ) 88 | include_directories( ${CMAKE_SOURCE_DIR}/include/dpp ) 89 | include_directories( ${CMAKE_SOURCE_DIR}/include/dpp/basic ) 90 | include_directories( ${CMAKE_SOURCE_DIR}/include/dpp/planner ) 91 | include_directories( ${CMAKE_SOURCE_DIR}/include/dpp/planalg ) 92 | 93 | ################################ 94 | # Settings and Compiler Flags 95 | ################################ 96 | # Set c++11 (c++0x) 97 | include(CheckCXXCompilerFlag) 98 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 99 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 100 | if(COMPILER_SUPPORTS_CXX11) 101 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 102 | elseif(COMPILER_SUPPORTS_CXX0X) 103 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 104 | else() 105 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 106 | endif() 107 | 108 | # Make binaries output in the same directory 109 | #set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) 110 | 111 | ## Compiler flags 112 | # OSX, TODO add if statement 113 | add_definitions("-fno-pie") 114 | 115 | # Debug flags 116 | if ( CMAKE_BUILD_TYPE MATCHES Debug ) 117 | message( "Compiling DPP in debug mode." ) 118 | set(GCC_DPP_DEBUG_COMPILE_FLAGS "-DDPP_DEBUG") 119 | set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GCC_DPP_DEBUG_COMPILE_FLAGS}" ) 120 | 121 | ## Print debug information about compiler 122 | # the compiler used for C files 123 | MESSAGE( STATUS "CMAKE_C_COMPILER: " ${CMAKE_C_COMPILER} ) 124 | 125 | # the compiler used for C++ files 126 | MESSAGE( STATUS "CMAKE_CXX_COMPILER: " ${CMAKE_CXX_COMPILER} ) 127 | 128 | # if the compiler is a variant of gcc, this should be set to 1 129 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCC: " ${CMAKE_COMPILER_IS_GNUCC} ) 130 | 131 | # if the compiler is a variant of g++, this should be set to 1 132 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCXX : " ${CMAKE_COMPILER_IS_GNUCXX} ) 133 | 134 | MESSAGE( STATUS "CXX Compiler flags: " ${CMAKE_CXX_FLAGS} ) 135 | 136 | MESSAGE( STATUS "Include paths:") 137 | 138 | get_property(dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) 139 | foreach(dir ${dirs}) 140 | message(STATUS "dir='${dir}'") 141 | endforeach() 142 | 143 | endif ( CMAKE_BUILD_TYPE MATCHES Debug ) 144 | 145 | ################################ 146 | # CMake Project Subdirectories 147 | ################################ 148 | ## Add subdirectories 149 | add_subdirectory( src ) 150 | add_subdirectory( test ) 151 | add_subdirectory( examples ) 152 | 153 | -------------------------------------------------------------------------------- /HISTORY: -------------------------------------------------------------------------------- 1 | Project History 2 | 3 | * Jan 15, 2015 -- Wrote MATLAB code for toy problems. Had PTP and LTL, and attempted to solve both greedily (nearest neighbor in O(n^2) time) and optimally (linear programming approximation in O(n! * PTAS(n)) time -- naive). 4 | 5 | * May 15, 2015 -- Added test harnesses and DubinsPath library to simulate trajectories. This was used to create plots and to check against the length obtained with a DubinsShortestPathLength algorithm. 6 | 7 | * Sept 7, 2015 -- Started C++ library for solving Dubins ATSP, first with Boost (too complicated) then with to OGDF. Wrote a NearestNeighbor algorithm, and wrote a DubinsShortestPathLength function to populate a weighted adjacency matrix. Also started a simple TSPLib-IO library. 8 | 9 | * Sept 9, 2015 -- Working on converting LKH C project to a C++ library. Next step is to convert DATSP project into library and write a mex function. Then, write some scripts in MATLAB to analyze the results. -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License 2 | Copyright (c) 2015 UCSC Autonomous Systems Lab 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | The above copyright notice and this permission notice shall be included in 10 | all copies or substantial portions of the Software. 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Dubins Path Planner Library for C++ 2 | 3 | 4 | * Website: http://dagoodma.github.io/dppl_code 5 | 6 | * Code: https://github.com/dagoodma/dppl_code 7 | 8 | * QGC Interface Code: https://github.com/dagoodma/qgroundcontrol/tree/dpp_addon_dtspascpp 9 | 10 | * MATLAB Analysis Code: https://github.com/dagoodma/dppl_matlab 11 | 12 | 13 | The goal of this project was to implement a path planning tool for fixed-wing UAVs, capable of efficiently planning two types of missions. The first is a point-to-point tour over a specific set of points. The second is coverage of an area for collecting data with an onboard sensor. Path length is used as the metric for efficiency in planning paths that conserve the vehicle’s endurance (battery power) allowing it to complete more tasks. This software can plan minimal length paths for accomplishing both types of missions, while also ensuring feasibility by considering the minimum turn radius constraints of the Dubins vehicle. 14 | 15 | We have created an open source path planning library called Dubins Path Planning Library (DPPL) to meet the goals outlined above. DPPL was written in C++ as a robust and extensible object-oriented library. Unit testing was implemented through the googletest framework to ensure robustness and aid in cross-platform implementation. DPPL can be used on Mac, Linux, and Windows operating systems. Additional software for plotting and analysis of test data was created for MATLAB (see the MATLAB repo: [DPPL MATLAB](https://github.com/dagoodma/dppl_matlab "dppl_matlab"). Some existing tools and software libraries were utilized in implementing DPPL: the Open Graph Drawing Framework (OGDF) was used for representing graphs and networks, and for reading and writing files in Graph Markup Language (GML) format. Eigen, a C++ template library for linear algebra, was used in some algorithms to simplify computations. For solving ETSP and ATSP tours, we used Helsgaun’s implementation of Lin-Kernighan heuristic, which is a C-based program called LKH. These dependencies will need to be downloaded and compiled separately and placed in the lib/ folder. 16 | 17 | ## Requirements 18 | 19 | DPPL requires a number of libraries. Most of these are included as Git submodules under the `lib/` directory, and can be fetched automatically (see the [Getting Started](#getting-started) section for instructions). Each library will need to be compiled separately before you can compile DPPL. Here's a condensed list of libraries required by DPPL (for a more complete list that includes more information and version numbers, see: [lib/README.md](lib/README.md)): 20 | 21 | * [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page) 22 | * [LKH](http://www.akira.ruc.dk/~keld/research/LKH/) 23 | * [cxxopts](https://github.com/jarro2783/cxxopts) 24 | * [OGDF](http://ogdf.net/doku.php) 25 | * [dubins-curves](https://github.com/dagoodma/Dubins-Curves/tree/cpp) 26 | * [stacktrace.h](lib/stacktrack) 27 | 28 | Here is a list of toolchain requirements (mostly imposed by OGDF): 29 | 30 | * CMake 3.1+ 31 | * C++11 compliant compiler 32 | * gcc 4.8+ 33 | * clang 3.5+ 34 | * Microsoft Visual C++ 2015+ 35 | * GNU Make (in most cases) 36 | 37 | ## Getting Started 38 | 39 | This section explains how to obtain everything needed to use DPPL. 40 | 41 | First, clone this Git repo recursively to download all of the submodules (there's a lot of dependencies that are grabbed this way--see the [.gitmodules](.gitmodules) file for the list of submodules, and [lib/README.md](lib/README.md) for additional information about dependencies): 42 | 43 | git clone --recursive https://github.com/dagoodma/dppl_code.git 44 | 45 | If you've already cloned this repo but you still need to download the submodules (for example if your `matlab/` folder is empty), then you should run the following commands in your local working copy: 46 | 47 | git submodule init 48 | git submodule update --recursive —remote 49 | 50 | Once you have downloaded DPPL and all of the submodules, you will still need to manually download and compile [LKH 2.0.7](http://www.akira.ruc.dk/~keld/research/LKH/LKH-2.0.7.tgz) and [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page)--a header-only library that should be available through your system's package manager (on Mac OS X, we recommend [Homebrew](http://brew.sh/index.html): `brew install eigen`). Extract and compile LKH into `lib/LKH` or `lib/LKH-2.0.7`. You will also need to compile OGDF and Dubins-Curves (requires [SCons](http://scons.org/), which should be available through your system's package manager). 51 | 52 | Now that you have DPPL and all required dependencies, you are ready to compile DPPL itself. We use [CMake](https://cmake.org/) as our build system. Thus you will want to create a new folder (we'll create a `build/` folder inside our local working repo), change into that folder, run `cmake `, and `make`: 53 | 54 | mkdir build 55 | cd build 56 | cmake ../ 57 | make 58 | 59 | ## QGroundControl Interface 60 | 61 | An interface was designed in the open source ground station software called QGroundControl (QGC). The interface allows users of QGC to plan efficient waypoint and coverage missions in real time on a map, and then send them to a vehicle controlled by an autopilot. QGC was used as the ground station when experimentally verifying DPPL using a fixed-wing UAV. Find the branch here: [DPPL QGC Interface](https://github.com/dagoodma/qgroundcontrol/tree/dpp_addon_dtspascpp "dpp_addon_dtspascpp (Branch)"). 62 | 63 | 64 | ## License 65 | 66 | DPPL is licensed under the terms of the MIT license. See LICENSE for more details. 67 | 68 | ## Author 69 | 70 | David Goodman (dagoodma at geemail dot com) 71 | 72 | -------------------------------------------------------------------------------- /cmake/Modules/FindCxxopts.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find cxxopts 2 | # Once done this will define 3 | # CXXOPTS_FOUND - System has cxxopts 4 | # CXXOPTS_INCLUDE_DIRS - The cxxopts include directories 5 | 6 | find_path(CXXOPTS_INCLUDE_DIR cxxopts.hpp 7 | PATHS: "lib/cxxopts/src" ) 8 | 9 | set(CXXOPTS_INCLUDE_DIRS ${CXXOPTS_INCLUDE_DIR} ) 10 | 11 | include(FindPackageHandleStandardArgs) 12 | # handle the QUIETLY and REQUIRED arguments and set CXXOPTS_FOUND to TRUE 13 | # if all listed variables are TRUE 14 | find_package_handle_standard_args(Cxxopts DEFAULT_MSG 15 | CXXOPTS_INCLUDE_DIR) 16 | 17 | mark_as_advanced(CXXOPTS_INCLUDE_DIR) -------------------------------------------------------------------------------- /cmake/Modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /cmake/Modules/FindOgdf.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find OGDF and COIN 2 | # TODO fixme 3 | # Once done this will define 4 | # OGDF_FOUND - System has ogdf 5 | # OGDF_INCLUDE_DIRS - The ogdf include directories 6 | # OGDF_LIBRARIES - The libraries needed to use ogdf 7 | # COIN_FOUND - System has coin 8 | # COIN_INCLUDE_DIRS - The coin include directories 9 | # COIN_LIBRARIES - The libraries needed to use coin 10 | 11 | # Find includes for ogdf and coin 12 | find_path(OGDF_INCLUDE_DIR ogdf/basic/basic.h coin/Coin_C_defines.h 13 | PATHS: "lib/ogdf/include" ) 14 | 15 | # Find OGDF lib 16 | find_library(OGDF_LIBRARY_DEBUG NAMES ogdf 17 | HINTS lib/ogdf 18 | PATH_SUFFIXES _debug ) 19 | find_library(OGDF_LIBRARY_RELEASE NAMES ogdf 20 | HINTS lib/ogdf 21 | PATH_SUFFIXES _release ) 22 | 23 | # Find COIN lib 24 | find_library(COIN_LIBRARY_DEBUG NAMES coin 25 | HINTS lib/ogdf 26 | PATH_SUFFIXES _debug ) 27 | find_library(COIN_LIBRARY_RELEASE NAMES coin 28 | HINTS lib/ogdf 29 | PATH_SUFFIXES _release ) 30 | 31 | # Set debug and release versions 32 | set( OGDF_LIBRARY debug ${OGDF_LIBRARY_DEBUG} 33 | optimized ${OGDF_LIBRARY_RELEASE} ) 34 | 35 | set( COIN_LIBRARY debug ${COIN_LIBRARY_DEBUG} 36 | optimized ${COIN_LIBRARY_RELEASE} ) 37 | 38 | message(STATUS "OGDF lib: " ${OGDF_LIBRARY}) 39 | message(STATUS "COin lib: " ${COIN_LIBRARY}) 40 | 41 | # set variables 42 | set(OGDF_LIBRARIES ${OGDF_LIBRARY} ) 43 | set(OGDF_LIBRARIES ${OGDF_LIBRARIES} ${COIN_LIBRARY} ) 44 | 45 | set(OGDF_INCLUDE_DIRS ${OGDF_INCLUDE_DIR} ) 46 | 47 | message(STATUS "Here with: " ${OGDF_LIBRARIES}) 48 | message(STATUS "Here with: " ${OGDF_INCLUDE_DIRS}) 49 | 50 | include(FindPackageHandleStandardArgs) 51 | # handle the QUIETLY and REQUIRED arguments and set OGDF_FOUND to TRUE 52 | # if all listed variables are TRUE 53 | find_package_handle_standard_args(Ogdf DEFAULT_MSG 54 | OGDF_LIBRARY OGDF_INCLUDE_DIR) 55 | 56 | mark_as_advanced(OGDF_INCLUDE_DIR OGDF_LIBRARY COIN_LIBRARY ) 57 | -------------------------------------------------------------------------------- /cmake/Modules/FindStacktrace.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find stacktrace 2 | # Once done this will define 3 | # STACKTRACE_FOUND - System has cxxopts 4 | # STACKTRACE_INCLUDE_DIRS - The cxxopts include directories 5 | 6 | find_path(STACKTRACE_INCLUDE_DIR stacktrace.h 7 | PATHS: "lib/stacktrace" ) 8 | 9 | set(STACKTRACE_INCLUDE_DIRS ${STACKTRACE_INCLUDE_DIR} ) 10 | 11 | include(FindPackageHandleStandardArgs) 12 | # handle the QUIETLY and REQUIRED arguments and set CXXOPTS_FOUND to TRUE 13 | # if all listed variables are TRUE 14 | find_package_handle_standard_args(Stacktrace DEFAULT_MSG 15 | STACKTRACE_INCLUDE_DIR) 16 | 17 | mark_as_advanced(STACKTRACE_INCLUDE_DIR) -------------------------------------------------------------------------------- /data/experiment/2015.11.07_phoenixflight1_3tours.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dagoodma/dppl_code/f05333b55fb2cb073bb572562726f8b711df54ad/data/experiment/2015.11.07_phoenixflight1_3tours.png -------------------------------------------------------------------------------- /data/experiment/2015.11.07_phoenixflight1_tour_notes.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dagoodma/dppl_code/f05333b55fb2cb073bb572562726f8b711df54ad/data/experiment/2015.11.07_phoenixflight1_tour_notes.md -------------------------------------------------------------------------------- /data/experiment/2015.11.07_phoenixtour_notes.md: -------------------------------------------------------------------------------- 1 | # 2015.11.10 Verification Flight1 for Waypoint Sequence Planner 2 | 3 | On 2015.11.10 at around 11:30 AM, Sharon and I launched the Phoenix (tail pusher) small 4 | fixed-wing aircraft. The goal was to conduct a test flight, using the optimal tour 5 | sequence generated with the path planner. 6 | 7 | We did about 3 tours through the points before the plane crashed. Sharon said he's had 8 | problems with the battery dying on the Phoenix. The best tour (closest to the 9 | planned) path length should be calculated for comparison. 10 | 11 | Looking at the data, it appears the strong South by Southwestarly wind may have 12 | prevented the plane from reaching point 3 and especially point 4. However, Sharon 13 | informed me that he does not have a radius of acceptance feature enabled. So 14 | this may be due to GPS measruement error. The error is within the 10-20 m range 15 | from waypoints 3 and 4. 16 | 17 | The minimum distance to each waypoint for each tour track should be calculated 18 | and compared as a measure of error. 19 | 20 | ## Scenario Data 21 | 22 | The origin is: 23 | 24 | [36.988505,-122.0509133,0] 25 | 26 | The points (local coordinates recorded in 2015.11.07_east4wp_local.csv) are: 27 | 28 | 14.1924, 91.5624 29 | -23.7207, 171.914 30 | -216.686, 189.736 31 | -236.224, 47.4646 32 | 33 | With the start heading at 0 rad (North), and initial position: 34 | 35 | 0, 0 36 | 37 | The headings should be computed (never saved) with the path planner for generating 38 | expected Dubins paths for comparison. 39 | 40 | -------------------------------------------------------------------------------- /data/experiment/2015.11.07_ptp_tour.csv: -------------------------------------------------------------------------------- 1 | x,y,phi,distanceFromLast 2 | 0,0,0.0,0,0 3 | 14.1924,91.5624,5.84232,93.443 4 | -23.7207,171.914,5.84232,88.847 5 | -216.686,189.736,3.27807,208.14 6 | -236.224,47.4646,3.27807,143.607 -------------------------------------------------------------------------------- /data/experiment/2015.11.07_ptp_tour.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 14.1924 25 | y 91.5624 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x -23.7207 37 | y 171.914 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x -216.686 49 | y 189.736 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | node [ 57 | id 5 58 | label "5" 59 | graphics [ 60 | x -236.224 61 | y 47.4646 62 | type "oval" 63 | ] 64 | LabelGraphics [ 65 | type "text" 66 | ] 67 | ] 68 | ] 69 | -------------------------------------------------------------------------------- /data/experiment/2015.11.18_cpp_polygon.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | comment "This is a polygon for example 1 with r=23, w=160, initial position=(0,0,0). Minimum width=278.494, orientation angle=105.932 deg" 3 | id 0 4 | version 0 5 | graphics [ 6 | ] 7 | LabelGraphics [ 8 | ] 9 | node [ 10 | id 1 11 | label "1" 12 | graphics [ 13 | x 26.6357 14 | y 135.882 15 | type "oval" 16 | ] 17 | LabelGraphics [ 18 | type "text" 19 | ] 20 | ] 21 | node [ 22 | id 2 23 | label "2" 24 | graphics [ 25 | x -166.484 26 | y 146.488 27 | type "oval" 28 | ] 29 | LabelGraphics [ 30 | type "text" 31 | ] 32 | ] 33 | node [ 34 | id 3 35 | label "3" 36 | graphics [ 37 | x -195.627 38 | y -99.1082 39 | type "oval" 40 | ] 41 | LabelGraphics [ 42 | type "text" 43 | ] 44 | ] 45 | node [ 46 | id 4 47 | label "4" 48 | graphics [ 49 | x -71.6223 50 | y -331.982 51 | type "oval" 52 | ] 53 | LabelGraphics [ 54 | type "text" 55 | ] 56 | ] 57 | node [ 58 | id 5 59 | label "5" 60 | graphics [ 61 | x 139.126 62 | y -258.175 63 | type "oval" 64 | ] 65 | LabelGraphics [ 66 | type "text" 67 | ] 68 | ] 69 | ] 70 | 71 | -------------------------------------------------------------------------------- /data/experiment/2015.11.18_cpp_tour.csv: -------------------------------------------------------------------------------- 1 | x,y,phi,distanceFromLast 2 | 0,0,0.0,0,0 3 | -57.8852,140.524,2.8635,215.5938 4 | -13.965,-13.3296,2.8635,160.0000 5 | 29.9552,-167.183,2.8635,160.0000 6 | 63.4918,-284.663,2.8635,122.1723 7 | -109.926,-260.05,6.0051,202.4018 8 | -153.846,-106.196,6.0051,160.0000 9 | -183.953,-0.730202,6.0051,109.6771 -------------------------------------------------------------------------------- /data/experiment/2015.11.18_phoenixflight2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dagoodma/dppl_code/f05333b55fb2cb073bb572562726f8b711df54ad/data/experiment/2015.11.18_phoenixflight2.png -------------------------------------------------------------------------------- /data/experiment/phoenixflight1Data.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | %% Parameters 5 | localWaypointFilename='2015.11.07_east4wp_local.csv'; 6 | dataRawLogFilename='2015.11.07_phoenixflight1_gpsraw.csv'; 7 | dataLogFilename='2015.11.07_phoenixflight1_gps.csv'; 8 | 9 | origin = [36.988505,-122.0509133,0]; % from raw telemetry data 10 | initialPosition = [0, 0]; 11 | initialHeading = 0.0; % rad 12 | [dataLocal] = csvread(localWaypointFilename, 1); 13 | %V = [14.1924, 91.5624;... 14 | % -23.7207, 171.914;... 15 | % -216.686, 189.736;... 16 | % -236.224, 47.4646]; 17 | V = [dataLocal(:,2), dataLocal(:,3)]; 18 | 19 | 20 | %% Add dependencies 21 | addpath('../../matlab', '../../matlab/lib'); 22 | 23 | %% Open and the convert the data 24 | [dataGeo, ~] = readCsvGps(dataLogFilename); 25 | [dataGeoRaw, ~] = readCsvGps(dataRawLogFilename,1); 26 | [dataNed] = convertGpsData(dataGeo(:,2:4), origin); 27 | [dataNedRaw] = convertGpsData(dataGeoRaw(:,2:4), origin); 28 | 29 | % Split tour by position indices (found experimentally) 30 | % For raw data 31 | % indTour1=[1957:2217]; 32 | % indTour2=[2218:2475]; 33 | % indTour3=[2476:2708]; 34 | indTour1=[1957:2217]; 35 | indTour2=[2218:2475]; 36 | indTour3=[2476:2708]; 37 | 38 | %% Plotting 39 | figure(); 40 | %plot(dataNed(indTour1,2), dataNed(indTour1,1), 'k+'); 41 | plot(dataNed(:,2), dataNed(:,1), 'k+'); 42 | ylabel('North [m]'); 43 | xlabel('East [m]'); 44 | hold on; 45 | % plot(dataNed(indTour2,2), dataNed(indTour2,1), 'bx'); 46 | % plot(dataNed(indTour3,2), dataNed(indTour3,1), 'mo'); 47 | 48 | %plot(V(:,1), V(:,2), 'go', 'MarkerFaceColor', 'g'); 49 | plot(V(:,1), V(:,2), '*', 'MarkerSize', 10, 'Color', [1 0 0]); 50 | for i=1:length(V) 51 | text(V(i,1)+2.5, V(i,2)+2.0, sprintf('%d',i),'FontSize',11); 52 | end 53 | 54 | plot(initialPosition(1),initialPosition(2),'ro', 'MarkerFaceColor', 'r'); 55 | 56 | legend('Tour 1', 'Tour 2', 'Tour 3', 'Waypoint','Origin') 57 | title('Three Tours with 20 [m] Turn Radius') 58 | hold off; 59 | 60 | %% Compare raw and estimated position 61 | figure(); 62 | subplot(2,1,1); 63 | plot(dataGeoRaw(:,1), dataNedRaw(:,1), 'k'); 64 | hold on; 65 | plot(dataGeo(:,1), dataNed(:,1), '--g'); 66 | xlabel('Time [s]') 67 | ylabel('North Position [m]') 68 | legend('Measured Position', 'Estimated Position') 69 | hold off; 70 | 71 | subplot(2,1,2); 72 | plot(dataGeoRaw(:,1), dataNedRaw(:,2), 'k'); 73 | hold on; 74 | plot(dataGeo(:,1), dataNed(:,2), '--g'); 75 | xlabel('Time [s]') 76 | ylabel('East Position [m]') 77 | legend('Measured Position', 'Estimated Position') 78 | hold off; 79 | -------------------------------------------------------------------------------- /data/experiment/phoenixflight2.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | %% Parameters 5 | %localWaypointFilename='2015.11.07_east4wp_local.csv'; 6 | dataLogFilename='2015.11.18_phoenixflight2_gps.csv'; 7 | 8 | origin = [36.988505,-122.0509133,0]; % from raw telemetry data 9 | initialPosition = [0, 0]; 10 | initialHeading = 0.0; % rad 11 | %[dataLocal] = csvread(localWaypointFilename, 1); 12 | V= [... 13 | -57.8852 ,140.524;... 14 | -13.965 ,-13.3296;... 15 | 29.9552 ,-167.183;... 16 | 63.4918 ,-284.663;... 17 | -109.926 , -260.05;... 18 | -153.846 , -106.196;... 19 | -183.953, -0.730202]; 20 | 21 | %V = [dataLocal(:,2), dataLocal(:,3)]; 22 | 23 | 24 | %% Add dependencies 25 | addpath('../../matlab', '../../matlab/lib'); 26 | 27 | %% Open and the convert the data 28 | [dataGeo, ~] = readCsvGps(dataLogFilename); 29 | [dataNed] = convertGpsData(dataGeo(:,2:4), origin); 30 | 31 | % Split tour by position indices (found experimentally) 32 | % For raw data 33 | % indTour1=[1957:2217]; 34 | % indTour2=[2218:2475]; 35 | % indTour3=[2476:2708]; 36 | indTour1=[1957:2217]; 37 | indTour2=[2218:2475]; 38 | indTour3=[2476:2708]; 39 | 40 | ind = [5000:6700]; 41 | 42 | %% Plotting 43 | figure(); 44 | plot(dataNed(ind,2), dataNed(ind,1), 'k-'); 45 | ylabel('North [m]'); 46 | xlabel('East [m]'); 47 | hold on; 48 | 49 | %plot(V(:,1), V(:,2), 'go', 'MarkerFaceColor', 'g'); 50 | plot(V(:,1), V(:,2), '*', 'MarkerSize', 10, 'Color', [1 0 0]); 51 | for i=1:length(V) 52 | text(V(i,1)+2.5, V(i,2)+2.0, sprintf('%d',i),'FontSize',11); 53 | end 54 | 55 | plot(initialPosition(1),initialPosition(2),'ro', 'MarkerFaceColor', 'r'); 56 | axis square; 57 | 58 | %legend('Tour 1', 'Tour 2', 'Tour 3', 'Waypoint','Origin') 59 | %title('Three Tours with 20 [m] Turn Radius') 60 | hold off; 61 | -------------------------------------------------------------------------------- /data/test/cpp/examplePolygon1.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | comment "This is a polygon for example 1 with r=23, w=160, initial position=(0,0,0). Minimum width=278.494, orientation angle=105.932 deg" 3 | id 0 4 | version 0 5 | graphics [ 6 | ] 7 | LabelGraphics [ 8 | ] 9 | node [ 10 | id 1 11 | label "1" 12 | graphics [ 13 | x 26.6357 14 | y 135.882 15 | type "oval" 16 | ] 17 | LabelGraphics [ 18 | type "text" 19 | ] 20 | ] 21 | node [ 22 | id 2 23 | label "2" 24 | graphics [ 25 | x -166.484 26 | y 146.488 27 | type "oval" 28 | ] 29 | LabelGraphics [ 30 | type "text" 31 | ] 32 | ] 33 | node [ 34 | id 3 35 | label "3" 36 | graphics [ 37 | x -195.627 38 | y -99.1082 39 | type "oval" 40 | ] 41 | LabelGraphics [ 42 | type "text" 43 | ] 44 | ] 45 | node [ 46 | id 4 47 | label "4" 48 | graphics [ 49 | x -71.6223 50 | y -331.982 51 | type "oval" 52 | ] 53 | LabelGraphics [ 54 | type "text" 55 | ] 56 | ] 57 | node [ 58 | id 5 59 | label "5" 60 | graphics [ 61 | x 139.126 62 | y -258.175 63 | type "oval" 64 | ] 65 | LabelGraphics [ 66 | type "text" 67 | ] 68 | ] 69 | ] 70 | 71 | -------------------------------------------------------------------------------- /data/test/cpp/isoscelesTrapezoid.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 10.0 25 | y 12.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x -10.0 37 | y 12.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x -8.0 49 | y 0.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | ] 57 | -------------------------------------------------------------------------------- /data/test/cpp/isoscelesTriangle.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 6.0 13 | y 2.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 4.0 25 | y 4.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x 2.0 37 | y 2.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | ] 45 | -------------------------------------------------------------------------------- /data/test/cpp/nonConvexPolygon.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x -10.0 25 | y 0.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x -10.0 37 | y -10.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 10.0 49 | y -10.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | node [ 57 | id 5 58 | label "5" 59 | graphics [ 60 | x 10.0 61 | y -5.0 62 | type "oval" 63 | ] 64 | LabelGraphics [ 65 | type "text" 66 | ] 67 | ] 68 | node [ 69 | id 6 70 | label "6" 71 | graphics [ 72 | x 0.0 73 | y -5.0 74 | type "oval" 75 | ] 76 | LabelGraphics [ 77 | type "text" 78 | ] 79 | ] 80 | ] 81 | -------------------------------------------------------------------------------- /data/test/cpp/rectangle.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x -10.0 25 | y 0.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x -10.0 37 | y -20.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 0.0 49 | y -20.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | ] 57 | -------------------------------------------------------------------------------- /data/test/cpp/rightTriangle.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x -10.0 25 | y 10.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x -10.0 37 | y 0.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | ] 45 | -------------------------------------------------------------------------------- /data/test/cpp/square.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x -10.0 25 | y 0.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x -10.0 37 | y -10.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 0.0 49 | y -10.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | ] 57 | -------------------------------------------------------------------------------- /data/test/cpp/trapezoid.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x -10.0 25 | y 10.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x -10.0 37 | y -20.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 0.0 49 | y -10.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | ] 57 | -------------------------------------------------------------------------------- /data/test/dtsp/square-36wp.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 7.0 25 | y 7.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x 7.0 37 | y 10.5 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 7.0 49 | y 14.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | node [ 57 | id 5 58 | label "5" 59 | graphics [ 60 | x 7.0 61 | y 17.5 62 | type "oval" 63 | ] 64 | LabelGraphics [ 65 | type "text" 66 | ] 67 | ] 68 | node [ 69 | id 6 70 | label "6" 71 | graphics [ 72 | x 7.0 73 | y 21.0 74 | type "oval" 75 | ] 76 | LabelGraphics [ 77 | type "text" 78 | ] 79 | ] 80 | node [ 81 | id 7 82 | label "7" 83 | graphics [ 84 | x 7.0 85 | y 24.5 86 | type "oval" 87 | ] 88 | LabelGraphics [ 89 | type "text" 90 | ] 91 | ] 92 | node [ 93 | id 8 94 | label "8" 95 | graphics [ 96 | x 7.0 97 | y 28.0 98 | type "oval" 99 | ] 100 | LabelGraphics [ 101 | type "text" 102 | ] 103 | ] 104 | node [ 105 | id 9 106 | label "9" 107 | graphics [ 108 | x 10.5 109 | y 7.0 110 | type "oval" 111 | ] 112 | LabelGraphics [ 113 | type "text" 114 | ] 115 | ] 116 | node [ 117 | id 10 118 | label "10" 119 | graphics [ 120 | x 10.5 121 | y 10.5 122 | type "oval" 123 | ] 124 | LabelGraphics [ 125 | type "text" 126 | ] 127 | ] 128 | node [ 129 | id 11 130 | label "11" 131 | graphics [ 132 | x 10.5 133 | y 14.0 134 | type "oval" 135 | ] 136 | LabelGraphics [ 137 | type "text" 138 | ] 139 | ] 140 | node [ 141 | id 12 142 | label "12" 143 | graphics [ 144 | x 10.5 145 | y 17.5 146 | type "oval" 147 | ] 148 | LabelGraphics [ 149 | type "text" 150 | ] 151 | ] 152 | node [ 153 | id 13 154 | label "13" 155 | graphics [ 156 | x 10.5 157 | y 21.0 158 | type "oval" 159 | ] 160 | LabelGraphics [ 161 | type "text" 162 | ] 163 | ] 164 | node [ 165 | id 14 166 | label "14" 167 | graphics [ 168 | x 10.5 169 | y 24.5 170 | type "oval" 171 | ] 172 | LabelGraphics [ 173 | type "text" 174 | ] 175 | ] 176 | node [ 177 | id 15 178 | label "15" 179 | graphics [ 180 | x 10.5 181 | y 28.0 182 | type "oval" 183 | ] 184 | LabelGraphics [ 185 | type "text" 186 | ] 187 | ] 188 | node [ 189 | id 16 190 | label "16" 191 | graphics [ 192 | x 14.0 193 | y 7.0 194 | type "oval" 195 | ] 196 | LabelGraphics [ 197 | type "text" 198 | ] 199 | ] 200 | node [ 201 | id 17 202 | label "17" 203 | graphics [ 204 | x 14.0 205 | y 10.5 206 | type "oval" 207 | ] 208 | LabelGraphics [ 209 | type "text" 210 | ] 211 | ] 212 | node [ 213 | id 18 214 | label "18" 215 | graphics [ 216 | x 14.0 217 | y 14.0 218 | type "oval" 219 | ] 220 | LabelGraphics [ 221 | type "text" 222 | ] 223 | ] 224 | node [ 225 | id 19 226 | label "19" 227 | graphics [ 228 | x 14.0 229 | y 17.5 230 | type "oval" 231 | ] 232 | LabelGraphics [ 233 | type "text" 234 | ] 235 | ] 236 | node [ 237 | id 20 238 | label "20" 239 | graphics [ 240 | x 14.0 241 | y 21.0 242 | type "oval" 243 | ] 244 | LabelGraphics [ 245 | type "text" 246 | ] 247 | ] 248 | node [ 249 | id 21 250 | label "21" 251 | graphics [ 252 | x 14.0 253 | y 24.5 254 | type "oval" 255 | ] 256 | LabelGraphics [ 257 | type "text" 258 | ] 259 | ] 260 | node [ 261 | id 22 262 | label "22" 263 | graphics [ 264 | x 14.0 265 | y 28.0 266 | type "oval" 267 | ] 268 | LabelGraphics [ 269 | type "text" 270 | ] 271 | ] 272 | node [ 273 | id 23 274 | label "23" 275 | graphics [ 276 | x 17.5 277 | y 7.0 278 | type "oval" 279 | ] 280 | LabelGraphics [ 281 | type "text" 282 | ] 283 | ] 284 | node [ 285 | id 24 286 | label "24" 287 | graphics [ 288 | x 17.5 289 | y 10.5 290 | type "oval" 291 | ] 292 | LabelGraphics [ 293 | type "text" 294 | ] 295 | ] 296 | node [ 297 | id 25 298 | label "25" 299 | graphics [ 300 | x 17.5 301 | y 14.0 302 | type "oval" 303 | ] 304 | LabelGraphics [ 305 | type "text" 306 | ] 307 | ] 308 | node [ 309 | id 26 310 | label "26" 311 | graphics [ 312 | x 17.5 313 | y 17.5 314 | type "oval" 315 | ] 316 | LabelGraphics [ 317 | type "text" 318 | ] 319 | ] 320 | node [ 321 | id 27 322 | label "27" 323 | graphics [ 324 | x 17.5 325 | y 21.0 326 | type "oval" 327 | ] 328 | LabelGraphics [ 329 | type "text" 330 | ] 331 | ] 332 | node [ 333 | id 28 334 | label "28" 335 | graphics [ 336 | x 17.5 337 | y 24.5 338 | type "oval" 339 | ] 340 | LabelGraphics [ 341 | type "text" 342 | ] 343 | ] 344 | node [ 345 | id 29 346 | label "29" 347 | graphics [ 348 | x 17.5 349 | y 28.0 350 | type "oval" 351 | ] 352 | LabelGraphics [ 353 | type "text" 354 | ] 355 | ] 356 | node [ 357 | id 30 358 | label "30" 359 | graphics [ 360 | x 21.0 361 | y 7.0 362 | type "oval" 363 | ] 364 | LabelGraphics [ 365 | type "text" 366 | ] 367 | ] 368 | node [ 369 | id 31 370 | label "31" 371 | graphics [ 372 | x 24.5 373 | y 7.0 374 | type "oval" 375 | ] 376 | LabelGraphics [ 377 | type "text" 378 | ] 379 | ] 380 | node [ 381 | id 32 382 | label "32" 383 | graphics [ 384 | x 28.0 385 | y 7.0 386 | type "oval" 387 | ] 388 | LabelGraphics [ 389 | type "text" 390 | ] 391 | ] 392 | node [ 393 | id 33 394 | label "33" 395 | graphics [ 396 | x 31.5 397 | y 7.0 398 | type "oval" 399 | ] 400 | LabelGraphics [ 401 | type "text" 402 | ] 403 | ] 404 | ] 405 | -------------------------------------------------------------------------------- /data/test/dtsp/triangle-27wp.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 15.0 25 | y -20.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x 15.0 37 | y -16.5 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 15.0 49 | y -13.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | node [ 57 | id 5 58 | label "5" 59 | graphics [ 60 | x 15.0 61 | y -9.5 62 | type "oval" 63 | ] 64 | LabelGraphics [ 65 | type "text" 66 | ] 67 | ] 68 | node [ 69 | id 6 70 | label "6" 71 | graphics [ 72 | x 15.0 73 | y -6.0 74 | type "oval" 75 | ] 76 | LabelGraphics [ 77 | type "text" 78 | ] 79 | ] 80 | node [ 81 | id 7 82 | label "7" 83 | graphics [ 84 | x 15.0 85 | y -2.5 86 | type "oval" 87 | ] 88 | LabelGraphics [ 89 | type "text" 90 | ] 91 | ] 92 | node [ 93 | id 8 94 | label "8" 95 | graphics [ 96 | x 18.5 97 | y -16.5 98 | type "oval" 99 | ] 100 | LabelGraphics [ 101 | type "text" 102 | ] 103 | ] 104 | node [ 105 | id 9 106 | label "9" 107 | graphics [ 108 | x 18.5 109 | y -13.0 110 | type "oval" 111 | ] 112 | LabelGraphics [ 113 | type "text" 114 | ] 115 | ] 116 | node [ 117 | id 10 118 | label "10" 119 | graphics [ 120 | x 18.5 121 | y -9.5 122 | type "oval" 123 | ] 124 | LabelGraphics [ 125 | type "text" 126 | ] 127 | ] 128 | node [ 129 | id 11 130 | label "11" 131 | graphics [ 132 | x 18.5 133 | y -6.0 134 | type "oval" 135 | ] 136 | LabelGraphics [ 137 | type "text" 138 | ] 139 | ] 140 | node [ 141 | id 12 142 | label "12" 143 | graphics [ 144 | x 18.5 145 | y -2.5 146 | type "oval" 147 | ] 148 | LabelGraphics [ 149 | type "text" 150 | ] 151 | ] 152 | node [ 153 | id 13 154 | label "13" 155 | graphics [ 156 | x 22.0 157 | y -13.0 158 | type "oval" 159 | ] 160 | LabelGraphics [ 161 | type "text" 162 | ] 163 | ] 164 | node [ 165 | id 14 166 | label "14" 167 | graphics [ 168 | x 22.0 169 | y -9.5 170 | type "oval" 171 | ] 172 | LabelGraphics [ 173 | type "text" 174 | ] 175 | ] 176 | node [ 177 | id 15 178 | label "15" 179 | graphics [ 180 | x 22.0 181 | y -6.0 182 | type "oval" 183 | ] 184 | LabelGraphics [ 185 | type "text" 186 | ] 187 | ] 188 | node [ 189 | id 16 190 | label "16" 191 | graphics [ 192 | x 22.0 193 | y -2.5 194 | type "oval" 195 | ] 196 | LabelGraphics [ 197 | type "text" 198 | ] 199 | ] 200 | node [ 201 | id 17 202 | label "17" 203 | graphics [ 204 | x 25.5 205 | y -13.0 206 | type "oval" 207 | ] 208 | LabelGraphics [ 209 | type "text" 210 | ] 211 | ] 212 | node [ 213 | id 18 214 | label "18" 215 | graphics [ 216 | x 25.5 217 | y -9.5 218 | type "oval" 219 | ] 220 | LabelGraphics [ 221 | type "text" 222 | ] 223 | ] 224 | node [ 225 | id 19 226 | label "19" 227 | graphics [ 228 | x 25.5 229 | y -6.0 230 | type "oval" 231 | ] 232 | LabelGraphics [ 233 | type "text" 234 | ] 235 | ] 236 | node [ 237 | id 20 238 | label "20" 239 | graphics [ 240 | x 25.5 241 | y -2.5 242 | type "oval" 243 | ] 244 | LabelGraphics [ 245 | type "text" 246 | ] 247 | ] 248 | node [ 249 | id 21 250 | label "21" 251 | graphics [ 252 | x 29.0 253 | y -9.5 254 | type "oval" 255 | ] 256 | LabelGraphics [ 257 | type "text" 258 | ] 259 | ] 260 | node [ 261 | id 22 262 | label "22" 263 | graphics [ 264 | x 29.0 265 | y -6.0 266 | type "oval" 267 | ] 268 | LabelGraphics [ 269 | type "text" 270 | ] 271 | ] 272 | node [ 273 | id 23 274 | label "23" 275 | graphics [ 276 | x 29.0 277 | y -2.5 278 | type "oval" 279 | ] 280 | LabelGraphics [ 281 | type "text" 282 | ] 283 | ] 284 | node [ 285 | id 24 286 | label "24" 287 | graphics [ 288 | x 32.5 289 | y -6.0 290 | type "oval" 291 | ] 292 | LabelGraphics [ 293 | type "text" 294 | ] 295 | ] 296 | node [ 297 | id 25 298 | label "25" 299 | graphics [ 300 | x 32.5 301 | y -2.5 302 | type "oval" 303 | ] 304 | LabelGraphics [ 305 | type "text" 306 | ] 307 | ] 308 | node [ 309 | id 26 310 | label "26" 311 | graphics [ 312 | x 36.0 313 | y -6.0 314 | type "oval" 315 | ] 316 | LabelGraphics [ 317 | type "text" 318 | ] 319 | ] 320 | node [ 321 | id 27 322 | label "27" 323 | graphics [ 324 | x 36.0 325 | y -2.5 326 | type "oval" 327 | ] 328 | LabelGraphics [ 329 | type "text" 330 | ] 331 | ] 332 | node [ 333 | id 28 334 | label "28" 335 | graphics [ 336 | x 39.5 337 | y -2.5 338 | type "oval" 339 | ] 340 | LabelGraphics [ 341 | type "text" 342 | ] 343 | ] 344 | ] 345 | -------------------------------------------------------------------------------- /data/test/dtsp/triangle-8wp.dot: -------------------------------------------------------------------------------- 1 | graph { 2 | graph [graphics="[ ]", 3 | id=0, 4 | labelGraphics="[ ]", 5 | version=0 6 | ]; 7 | 1 [LabelGraphics="[ type \"text\" ]", 8 | name=1, 9 | pos="0.0,0.0", 10 | shape=oval]; 11 | 2 [LabelGraphics="[ type \"text\" ]", 12 | name=2, 13 | pos="150.0,0.0", 14 | shape=oval]; 15 | 3 [LabelGraphics="[ type \"text\" ]", 16 | name=3, 17 | pos="250.0,0.0", 18 | shape=oval]; 19 | 4 [LabelGraphics="[ type \"text\" ]", 20 | name=4, 21 | pos="350.0,0.0", 22 | shape=oval]; 23 | 5 [LabelGraphics="[ type \"text\" ]", 24 | name=5, 25 | pos="450.0,0.0", 26 | shape=oval]; 27 | 6 [LabelGraphics="[ type \"text\" ]", 28 | name=6, 29 | pos="150.0,-100.0", 30 | shape=oval]; 31 | 7 [LabelGraphics="[ type \"text\" ]", 32 | name=7, 33 | pos="250.0,-100.0", 34 | shape=oval]; 35 | 8 [LabelGraphics="[ type \"text\" ]", 36 | name=8, 37 | pos="350.0,-100.0", 38 | shape=oval]; 39 | 9 [LabelGraphics="[ type \"text\" ]", 40 | name=9, 41 | pos="150.0,-200.0", 42 | shape=oval]; 43 | } 44 | -------------------------------------------------------------------------------- /data/test/dtsp/triangle-8wp.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 150.0 25 | y 0.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x 250.0 37 | y 0.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 350.0 49 | y 0.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | node [ 57 | id 5 58 | label "5" 59 | graphics [ 60 | x 450.0 61 | y 0.0 62 | type "oval" 63 | ] 64 | LabelGraphics [ 65 | type "text" 66 | ] 67 | ] 68 | node [ 69 | id 6 70 | label "6" 71 | graphics [ 72 | x 150.0 73 | y -100.0 74 | type "oval" 75 | ] 76 | LabelGraphics [ 77 | type "text" 78 | ] 79 | ] 80 | node [ 81 | id 7 82 | label "7" 83 | graphics [ 84 | x 250.0 85 | y -100.0 86 | type "oval" 87 | ] 88 | LabelGraphics [ 89 | type "text" 90 | ] 91 | ] 92 | node [ 93 | id 8 94 | label "8" 95 | graphics [ 96 | x 350.0 97 | y -100.0 98 | type "oval" 99 | ] 100 | LabelGraphics [ 101 | type "text" 102 | ] 103 | ] 104 | node [ 105 | id 9 106 | label "9" 107 | graphics [ 108 | x 150.0 109 | y -200.0 110 | type "oval" 111 | ] 112 | LabelGraphics [ 113 | type "text" 114 | ] 115 | ] 116 | ] 117 | -------------------------------------------------------------------------------- /data/trash/square_polygon.dlm: -------------------------------------------------------------------------------- 1 | 3,0 2 | 9,0 3 | 9,-4 4 | 3,-4 5 | -------------------------------------------------------------------------------- /data/trash/square_polygon.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 3.0 13 | y -4.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 9.0 25 | y -4.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x 9.0 37 | y 0.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 3.0 49 | y 0.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | ] 57 | 58 | -------------------------------------------------------------------------------- /data/trash/testing.dot: -------------------------------------------------------------------------------- 1 | graph { 2 | graph [graphics="[ ]", 3 | id=0, 4 | labelGraphics="[ ]", 5 | version=0 6 | ]; 7 | 1 [LabelGraphics="[ type \"text\" ]", 8 | name=1, 9 | pos="90000.0,0.0", 10 | shape=oval]; 11 | 2 [LabelGraphics="[ type \"text\" ]", 12 | name=2, 13 | pos="150000.0,0.0", 14 | shape=oval]; 15 | 1 -- 2 [LabelGraphics="[ type \"text\" ]", 16 | lp="0,0"]; 17 | 3 [LabelGraphics="[ type \"text\" ]", 18 | name=3, 19 | pos="210000.0,0.0", 20 | shape=oval]; 21 | 1 -- 3 [LabelGraphics="[ type \"text\" ]", 22 | lp="0,0"]; 23 | 4 [LabelGraphics="[ type \"text\" ]", 24 | name=4, 25 | pos="270000.0,0.0", 26 | shape=oval]; 27 | 1 -- 4 [LabelGraphics="[ type \"text\" ]", 28 | lp="0,0"]; 29 | 5 [LabelGraphics="[ type \"text\" ]", 30 | name=5, 31 | pos="90000.0,-60000.0", 32 | shape=oval]; 33 | 1 -- 5 [LabelGraphics="[ type \"text\" ]", 34 | lp="0,0"]; 35 | 2 -- 1 [LabelGraphics="[ type \"text\" ]", 36 | lp="0,0"]; 37 | 2 -- 3 [LabelGraphics="[ type \"text\" ]", 38 | lp="0,0"]; 39 | 2 -- 4 [LabelGraphics="[ type \"text\" ]", 40 | lp="0,0"]; 41 | 2 -- 5 [LabelGraphics="[ type \"text\" ]", 42 | lp="0,0"]; 43 | 3 -- 1 [LabelGraphics="[ type \"text\" ]", 44 | lp="0,0"]; 45 | 3 -- 2 [LabelGraphics="[ type \"text\" ]", 46 | lp="0,0"]; 47 | 3 -- 4 [LabelGraphics="[ type \"text\" ]", 48 | lp="0,0"]; 49 | 3 -- 5 [LabelGraphics="[ type \"text\" ]", 50 | lp="0,0"]; 51 | 4 -- 1 [LabelGraphics="[ type \"text\" ]", 52 | lp="0,0"]; 53 | 4 -- 2 [LabelGraphics="[ type \"text\" ]", 54 | lp="0,0"]; 55 | 4 -- 3 [LabelGraphics="[ type \"text\" ]", 56 | lp="0,0"]; 57 | 4 -- 5 [LabelGraphics="[ type \"text\" ]", 58 | lp="0,0"]; 59 | 5 -- 1 [LabelGraphics="[ type \"text\" ]", 60 | lp="0,0"]; 61 | 5 -- 2 [LabelGraphics="[ type \"text\" ]", 62 | lp="0,0"]; 63 | 5 -- 3 [LabelGraphics="[ type \"text\" ]", 64 | lp="0,0"]; 65 | 5 -- 4 [LabelGraphics="[ type \"text\" ]", 66 | lp="0,0"]; 67 | } 68 | -------------------------------------------------------------------------------- /data/trash/testing.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 0.0 13 | y 0.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 150.0 25 | y 0.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x 250.0 37 | y 0.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | node [ 45 | id 4 46 | label "4" 47 | graphics [ 48 | x 350.0 49 | y 0.0 50 | type "oval" 51 | ] 52 | LabelGraphics [ 53 | type "text" 54 | ] 55 | ] 56 | node [ 57 | id 5 58 | label "5" 59 | graphics [ 60 | x 450.0 61 | y 0.0 62 | type "oval" 63 | ] 64 | LabelGraphics [ 65 | type "text" 66 | ] 67 | ] 68 | node [ 69 | id 6 70 | label "6" 71 | graphics [ 72 | x 150.0 73 | y -100.0 74 | type "oval" 75 | ] 76 | LabelGraphics [ 77 | type "text" 78 | ] 79 | ] 80 | ] 81 | -------------------------------------------------------------------------------- /data/trash/triangle_polygon.dlm: -------------------------------------------------------------------------------- 1 | 3,-4 2 | 9,0 3 | 3,0 4 | -------------------------------------------------------------------------------- /data/trash/triangle_polygon.gml: -------------------------------------------------------------------------------- 1 | graph [ 2 | id 0 3 | version 0 4 | graphics [ 5 | ] 6 | LabelGraphics [ 7 | ] 8 | node [ 9 | id 1 10 | label "1" 11 | graphics [ 12 | x 9.0 13 | y -12.0 14 | type "oval" 15 | ] 16 | LabelGraphics [ 17 | type "text" 18 | ] 19 | ] 20 | node [ 21 | id 2 22 | label "2" 23 | graphics [ 24 | x 27.0 25 | y 0.0 26 | type "oval" 27 | ] 28 | LabelGraphics [ 29 | type "text" 30 | ] 31 | ] 32 | node [ 33 | id 3 34 | label "3" 35 | graphics [ 36 | x 9.0 37 | y 0.0 38 | type "oval" 39 | ] 40 | LabelGraphics [ 41 | type "text" 42 | ] 43 | ] 44 | ] 45 | 46 | -------------------------------------------------------------------------------- /examples/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists 2 | #set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) 3 | 4 | ################################ 5 | # Binaries 6 | ################################ 7 | 8 | # Solve DTSP example binary 9 | ADD_EXECUTABLE( solveDtsp_example solveDtsp_example.cpp ) 10 | TARGET_LINK_LIBRARIES( solveDtsp_example ${OGDF_LIBRARIES} DPP ) 11 | 12 | # Solve DTSP simplified example binary 13 | ADD_EXECUTABLE( solveDtspSimple_example solveDtspSimple_example.cpp ) 14 | TARGET_LINK_LIBRARIES( solveDtspSimple_example ${OGDF_LIBRARIES} DPP ) 15 | 16 | -------------------------------------------------------------------------------- /examples/CMakeLists_example.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists 2 | cmake_minimum_required(VERSION 3.1) 3 | project (EXAMPLES CXX) 4 | 5 | find_library(OGDF ogdf) 6 | find_library(COIN coin) 7 | # PATHS ../../lib/ogdf/_debug) 8 | 9 | ## Set c++11 (c++0x) 10 | include(CheckCXXCompilerFlag) 11 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 12 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 13 | if(COMPILER_SUPPORTS_CXX11) 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 15 | elseif(COMPILER_SUPPORTS_CXX0X) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 17 | else() 18 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 19 | endif() 20 | 21 | ################################ 22 | # OGDF 23 | ################################ 24 | find_library(OGDF ogdf) 25 | find_library(COIN coin) 26 | 27 | include_directories( ${CMAKE_SOURCE_DIR}../../lib/ogdf/include ) 28 | if ( CMAKE_BUILD_TYPE MATCHES Debug ) 29 | link_directories( ${CMAKE_SOURCE_DIR}../../lib/ogdf/_debug ) 30 | else() 31 | link_directories( ${CMAKE_SOURCE_DIR}../../lib/ogdf/_release ) 32 | endif() 33 | 34 | 35 | #SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} -g" ) 36 | 37 | ## Build and Link Executables 38 | 39 | ADD_EXECUTABLE( manual manual.cpp ) 40 | ADD_EXECUTABLE( random random.cpp ) 41 | ADD_EXECUTABLE( sierpinski sierpinski.cpp ) 42 | 43 | TARGET_LINK_LIBRARIES( manual OGDF COIN ) 44 | TARGET_LINK_LIBRARIES( random OGDF COIN ) 45 | TARGET_LINK_LIBRARIES( sierpinski OGDF COIN ) 46 | 47 | ## Boost Examples 48 | #FIND_PACKAGE( Boost 1.40 COMPONENTS program_options REQUIRED ) 49 | #INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} ) 50 | # 51 | ##ADD_EXECUTABLE( dgaTest directed_graph_test.cpp ) 52 | #ADD_EXECUTABLE( graphVizTest write_graphviz.cpp ) 53 | #ADD_EXECUTABLE( graphVizReadTest read_graphviz.cpp ) 54 | # 55 | #TARGET_LINK_LIBRARIES( graphVizTest ${Boost_LIBRARIES} ) 56 | ##TARGET_LINK_LIBRARIES( dgaTest ${Boost_LIBRARIES} ) 57 | #TARGET_LINK_LIBRARIES( graphVizReadTest ${Boost_LIBRARIES} ) 58 | 59 | # the compiler used for C files 60 | MESSAGE( STATUS "CMAKE_C_COMPILER: " ${CMAKE_C_COMPILER} ) 61 | 62 | # the compiler used for C++ files 63 | MESSAGE( STATUS "CMAKE_CXX_COMPILER: " ${CMAKE_CXX_COMPILER} ) 64 | 65 | # if the compiler is a variant of gcc, this should be set to 1 66 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCC: " ${CMAKE_COMPILER_IS_GNUCC} ) 67 | 68 | # if the compiler is a variant of g++, this should be set to 1 69 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCXX : " ${CMAKE_COMPILER_IS_GNUCXX} ) 70 | -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | # Example Code 2 | 3 | To build the example code, create a build directory and run cmake: 4 | 5 | mkdir build 6 | cd build 7 | cmake .. 8 | 9 | To build in debug mode, run: 10 | 11 | mkdir build-debug 12 | cd build-debug 13 | cmake .. -DCMAKE_BUILD_TYPE=Debug 14 | 15 | Examples output dot files to std::out. The can be viewed in graph-viz through the gui. You can also use the `dot` command to convert the files into images for viewing. Try this: 16 | 17 | ./dgaTest > test.dot && dot -Tpng test.dot > test.png && open test.png 18 | 19 | 20 | For gml files, try converting them to dot files: 21 | 22 | gml2gv -otest.dot test.gml 23 | -------------------------------------------------------------------------------- /examples/boost/directed_graph_test.cpp: -------------------------------------------------------------------------------- 1 | //======================================================================= 2 | // Copyright 2012 3 | // Authors: David Doria 4 | // 5 | // Distributed under the Boost Software License, Version 1.0. (See 6 | // accompanying file LICENSE_1_0.txt or copy at 7 | // http://www.boost.org/LICENSE_1_0.txt) 8 | //======================================================================= 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include // A subclass to provide reasonable arguments to adjacency_list for a typical directed graph 15 | 16 | #include 17 | 18 | using namespace boost; 19 | 20 | int main(int,char*[]) 21 | { 22 | // directed_graph is a subclass of adjacency_list which gives you object oriented access to functions 23 | // like add_vertex and add_edge, which makes the code easier to understand. However, it hard codes many 24 | // of the template parameters, so it is much less flexible. 25 | 26 | typedef boost::property EdgeWeightProperty; 27 | typedef boost::directed_graph Graph; 28 | Graph g; 29 | boost::graph_traits::vertex_descriptor v0 = g.add_vertex(); 30 | boost::graph_traits::vertex_descriptor v1 = g.add_vertex(); 31 | 32 | EdgeWeightProperty ew = 3.1; 33 | g.add_edge(v0, v1, ew); 34 | 35 | write_graphviz(std::cout, g, make_label_writer(get(&Edge::weight, g))); 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /examples/boost/dubins_graph_test.cpp: -------------------------------------------------------------------------------- 1 | struct Configuration 2 | { 3 | double x, y, heading; 4 | }; 5 | 6 | struct Path 7 | { 8 | double cost; 9 | int source, destination; 10 | }; 11 | -------------------------------------------------------------------------------- /examples/boost/euclidean_tsp.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Compute the shortest Euclidean path for the Traveling Salesman Problem. 3 | * 4 | **/ 5 | 6 | using namespace boost; 7 | 8 | int main(int,char*[]) 9 | { 10 | 11 | } 12 | 13 | -------------------------------------------------------------------------------- /examples/boost/read_graphviz.cpp: -------------------------------------------------------------------------------- 1 | //======================================================================= 2 | // Copyright 2001 University of Notre Dame. 3 | // Author: Lie-Quan Lee 4 | // 5 | // This file is part of the Boost Graph Library 6 | // 7 | // You should have received a copy of the License Agreement for the 8 | // Boost Graph Library along with the software; see the file LICENSE. 9 | // If not, contact Office of Research, University of Notre Dame, Notre 10 | // Dame, IN 46556. 11 | // 12 | // Permission to modify the code and to distribute modified code is 13 | // granted, provided the text of this NOTICE is retained, a notice that 14 | // the code was modified is included with the above COPYRIGHT NOTICE and 15 | // with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE 16 | // file is distributed with the modified code. 17 | // 18 | // LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED. 19 | // By way of example, but not limitation, Licensor MAKES NO 20 | // REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY 21 | // PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS 22 | // OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS 23 | // OR OTHER RIGHTS. 24 | //======================================================================= 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | using boost::GraphvizDigraph; 35 | 36 | using namespace std; 37 | 38 | using namespace boost; 39 | 40 | template 41 | const std::string& 42 | vertex_label(const Vertex& u, const GraphvizDigraph& g) { 43 | boost::property_map::const_type 44 | va = boost::get(boost::vertex_attribute, g); 45 | return (*(va[u].find("label"))).second; 46 | } 47 | 48 | 49 | void print(GraphvizDigraph& g) { 50 | typedef GraphvizDigraph Graph; 51 | boost::graph_traits::vertex_iterator i, end; 52 | boost::graph_traits::out_edge_iterator ei, edge_end; 53 | for(boost::tie(i,end) = boost::vertices(g); i != end; ++i) { 54 | std::cout << vertex_label(*i, g) << " --> "; 55 | for (boost::tie(ei,edge_end) = boost::out_edges(*i, g); 56 | ei != edge_end; ++ei) 57 | std::cout << vertex_label(boost::target(*ei, g), g) << " "; 58 | std::cout << std::endl; 59 | } 60 | } 61 | 62 | /* 63 | This is to give an example of read graphviz file and create BGL graph. 64 | Then write the graph to the graphviz file. 65 | */ 66 | 67 | 68 | int main(int argc, char* argv[]) { 69 | std::cout << "This is an example to demonstrate how to read graphviz file" 70 | << std::endl 71 | << "and how to write graph to graphviz format." 72 | << std::endl 73 | << std::endl 74 | << std::endl; 75 | 76 | std::cout << "Usage: " << argv[0] << " .dot .dot " 77 | << std::endl; 78 | std::cout << "If only have one xxx.dot in command line," << std::endl 79 | << "the second dot is graphviz_test_new.dot by default." << std::endl 80 | << "If there is no input and output dot file in command line, " 81 | << "input is graphviz_test.dot and output graphviz_test_new.dot." << std::endl; 82 | 83 | std::string filename = "graphviz_test.dot"; 84 | 85 | if ( argc > 1 ) 86 | filename = argv[1]; 87 | 88 | GraphvizDigraph g; 89 | 90 | boost::read_graphviz(filename, g); 91 | 92 | print(g); 93 | 94 | const char* dot = "graphviz_test_new.dot"; 95 | if ( argc > 2 ) 96 | dot = argv[2]; 97 | 98 | boost::write_graphviz(dot, g); 99 | 100 | return 0; 101 | } 102 | -------------------------------------------------------------------------------- /examples/boost/test.dot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dagoodma/dppl_code/f05333b55fb2cb073bb572562726f8b711df54ad/examples/boost/test.dot -------------------------------------------------------------------------------- /examples/boost/write_graphviz.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include // A subclass to provide reasonable arguments to adjacency_list for a typical directed graph 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | enum files_e { dax_h, yow_h, boz_h, zow_h, foo_cpp, 10 | foo_o, bar_cpp, bar_o, libfoobar_a, 11 | zig_cpp, zig_o, zag_cpp, zag_o, 12 | libzigzag_a, killerapp, N }; 13 | const char* name[] = { "dax.h", "yow.h", "boz.h", "zow.h", "foo.cpp", 14 | "foo.o", "bar.cpp", "bar.o", "libfoobar.a", 15 | "zig.cpp", "zig.o", "zag.cpp", "zag.o", 16 | "libzigzag.a", "killerapp" }; 17 | 18 | int main(int,char*[]) 19 | { 20 | 21 | typedef pair Edge; 22 | Edge used_by[] = { 23 | Edge(dax_h, foo_cpp), Edge(dax_h, bar_cpp), Edge(dax_h, yow_h), 24 | Edge(yow_h, bar_cpp), Edge(yow_h, zag_cpp), 25 | Edge(boz_h, bar_cpp), Edge(boz_h, zig_cpp), Edge(boz_h, zag_cpp), 26 | Edge(zow_h, foo_cpp), 27 | Edge(foo_cpp, foo_o), 28 | Edge(foo_o, libfoobar_a), 29 | Edge(bar_cpp, bar_o), 30 | Edge(bar_o, libfoobar_a), 31 | Edge(libfoobar_a, libzigzag_a), 32 | Edge(zig_cpp, zig_o), 33 | Edge(zig_o, libzigzag_a), 34 | Edge(zag_cpp, zag_o), 35 | Edge(zag_o, libzigzag_a), 36 | Edge(libzigzag_a, killerapp) 37 | }; 38 | const int nedges = sizeof(used_by)/sizeof(Edge); 39 | int weights[nedges]; 40 | std::fill(weights, weights + nedges, 1); 41 | 42 | using namespace boost; 43 | 44 | typedef adjacency_list< vecS, vecS, directedS, 45 | property< vertex_color_t, default_color_type >, 46 | property< edge_weight_t, int > 47 | > Graph; 48 | Graph g(used_by, used_by + nedges, weights, N); 49 | 50 | write_graphviz(std::cout, g, make_label_writer(name)); 51 | } 52 | -------------------------------------------------------------------------------- /examples/ogdf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists 2 | cmake_minimum_required(VERSION 3.1) 3 | project (EXAMPLES CXX) 4 | 5 | find_library(OGDF ogdf) 6 | find_library(COIN coin) 7 | # PATHS ../../lib/ogdf/_debug) 8 | 9 | ## Set c++11 (c++0x) 10 | include(CheckCXXCompilerFlag) 11 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 12 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 13 | if(COMPILER_SUPPORTS_CXX11) 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 15 | elseif(COMPILER_SUPPORTS_CXX0X) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 17 | else() 18 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 19 | endif() 20 | 21 | ################################ 22 | # OGDF 23 | ################################ 24 | find_library(OGDF ogdf) 25 | find_library(COIN coin) 26 | 27 | include_directories( ${CMAKE_SOURCE_DIR}/../../lib/ogdf/include ) 28 | if ( CMAKE_BUILD_TYPE MATCHES Debug ) 29 | link_directories( ${CMAKE_SOURCE_DIR}/../../lib/ogdf/_debug ) 30 | else() 31 | link_directories( ${CMAKE_SOURCE_DIR}/../../lib/ogdf/_release ) 32 | endif() 33 | 34 | 35 | #SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} -g" ) 36 | 37 | ## Build and Link Executables 38 | 39 | ADD_EXECUTABLE( manual manual.cpp ) 40 | ADD_EXECUTABLE( random random.cpp ) 41 | ADD_EXECUTABLE( sierpinski sierpinski.cpp ) 42 | 43 | TARGET_LINK_LIBRARIES( manual OGDF COIN ) 44 | TARGET_LINK_LIBRARIES( random OGDF COIN ) 45 | TARGET_LINK_LIBRARIES( sierpinski OGDF COIN ) 46 | 47 | ## Boost Examples 48 | #FIND_PACKAGE( Boost 1.40 COMPONENTS program_options REQUIRED ) 49 | #INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} ) 50 | # 51 | ##ADD_EXECUTABLE( dgaTest directed_graph_test.cpp ) 52 | #ADD_EXECUTABLE( graphVizTest write_graphviz.cpp ) 53 | #ADD_EXECUTABLE( graphVizReadTest read_graphviz.cpp ) 54 | # 55 | #TARGET_LINK_LIBRARIES( graphVizTest ${Boost_LIBRARIES} ) 56 | ##TARGET_LINK_LIBRARIES( dgaTest ${Boost_LIBRARIES} ) 57 | #TARGET_LINK_LIBRARIES( graphVizReadTest ${Boost_LIBRARIES} ) 58 | 59 | # the compiler used for C files 60 | MESSAGE( STATUS "CMAKE_C_COMPILER: " ${CMAKE_C_COMPILER} ) 61 | 62 | # the compiler used for C++ files 63 | MESSAGE( STATUS "CMAKE_CXX_COMPILER: " ${CMAKE_CXX_COMPILER} ) 64 | 65 | # if the compiler is a variant of gcc, this should be set to 1 66 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCC: " ${CMAKE_COMPILER_IS_GNUCC} ) 67 | 68 | # if the compiler is a variant of g++, this should be set to 1 69 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCXX : " ${CMAKE_COMPILER_IS_GNUCXX} ) 70 | -------------------------------------------------------------------------------- /examples/ogdf/manual.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace ogdf; 6 | 7 | int main() 8 | { 9 | Graph G; 10 | GraphAttributes GA(G, GraphAttributes::nodeGraphics | GraphAttributes::edgeGraphics ); 11 | 12 | const int LEN = 11; 13 | for(int i = 1; i 2 | #include 3 | #include 4 | 5 | using namespace ogdf; 6 | 7 | int main() 8 | { 9 | Graph G; 10 | GraphAttributes GA(G, GraphAttributes::nodeGraphics | GraphAttributes::edgeGraphics ); 11 | 12 | const int LEN = 11; 13 | for(int i = 1; igraphOf() == m_pGraph), function operator[], 22 | file /usr/local/include/ogdf/basic/NodeArray.h, line 184. 23 | Abort trap: 6 24 | */ 25 | 26 | cout << "Here3\n"; 27 | //int xl = GA.x(left); 28 | cout << "Here1\n"; 29 | //xl = -5*(i+1); 30 | cout << "Here2\n"; 31 | GA.y(left) = -20*i; 32 | GA.width(left) = 10*(i+1); 33 | GA.height(left) = 15; 34 | 35 | node bottom = G.newNode(); 36 | cout << "Here3\n"; 37 | GA.x(bottom) = 20*(LEN-i); 38 | cout << "Here4\n"; 39 | GA.y(bottom) = 5*(LEN+1-i); 40 | GA.width(bottom) = 15; 41 | GA.height(bottom) = 10*(LEN+1-i); 42 | 43 | edge e = G.newEdge(left,bottom); 44 | DPolyline &p = GA.bends(e); 45 | p.pushBack(DPoint(10,-20*i)); 46 | p.pushBack(DPoint(20*(LEN-i),-10)); 47 | } 48 | 49 | //GraphIO::drawSVG(GA, "manual_graph.svg"); 50 | //GraphIO::writeGML(GA, "manual_graph.gml"); 51 | GraphIO::writeDOT(GA, "manual_graph.dot"); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /examples/ogdf/random.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace ogdf; 7 | 8 | int main() 9 | { 10 | Graph G; 11 | randomSimpleGraph(G, 10, 20); 12 | DfsAcyclicSubgraph DAS; 13 | DAS.callAndReverse(G); 14 | //GraphIO::drawSVG(G, "test.svg"); 15 | //GraphIO::writeGML(G, "test.gml"); 16 | GraphIO::writeDOT(G, "random_graph.dot"); 17 | 18 | return 0; 19 | } 20 | 21 | 22 | -------------------------------------------------------------------------------- /examples/ogdf/sierpinski.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | *This is the solution file for the Exercise 02 3 | * 4 | * @author Chameera Wijebandara 5 | * @email chameerawijebandara@gmail.com 6 | * 7 | * Windows 7 8 | * Vishual stuio 2010 9 | * OGDF Snapshot 2014-02-28 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | # define NODESIZE 10 22 | using namespace ogdf; 23 | 24 | bool sierpinksiGraph ( const int n ); 25 | void initSierpinksiGraph( Graph *G, GraphAttributes *GA, float points [3][2] ); 26 | void genarateSierpinksiGraph( Graph *G, GraphAttributes *GA, float parent[3][2], int count ); 27 | 28 | // main 29 | int main() 30 | { 31 | cout<<"Pleace enter number n (n>0) : "; 32 | int n; 33 | cin >> n; 34 | sierpinksiGraph(n); 35 | return 0; 36 | } 37 | //end of the main 38 | 39 | /* 40 | A function sierpinksiGraph that creates a Sierpinksi graph of order n, where n is a function parameter. 41 | and writes the final drawing to a SVG file 42 | graph generator for Sierpinski graphs 43 | */ 44 | 45 | bool sierpinksiGraph ( const int n )//number of nodes n and the number of edges m of the graph to be generated 46 | { 47 | if( n < 1 ) // if input is invalid 48 | { 49 | return false; 50 | } 51 | 52 | Graph G; // inti garaph 53 | GraphAttributes GA(G, GraphAttributes::nodeGraphics | 54 | GraphAttributes::nodeStyle | 55 | GraphAttributes::nodeLabel | 56 | GraphAttributes::edgeGraphics | 57 | GraphAttributes::edgeArrow | 58 | GraphAttributes::edgeStyle ); 59 | 60 | float points [ 3 ][ 2 ] = { 61 | { 0, 0 }, 62 | { 50*(n+3), 87*(n+3) }, 63 | { -50*(n+3), 87*(n+3) }}; // points of the outer triangerl 64 | 65 | initSierpinksiGraph( &G, &GA, points ); // init the Sierpinski graphs 66 | genarateSierpinksiGraph( &G, &GA, points, n-1 ); // recursively call pirates of the graph 67 | 68 | GraphIO::drawSVG( GA, "mst.svg" ); // write in to file 69 | 70 | return true; 71 | } 72 | 73 | /* 74 | initialize the outer triangle 75 | */ 76 | void initSierpinksiGraph(Graph *G, GraphAttributes *GA,float points [3][2]) 77 | { 78 | 79 | for(int i=0;i<3;i++) // 3 points in thetriangle 80 | { 81 | node v = G->newNode(); 82 | cout << "Here1" << GA->x(v) << "\n"; 83 | //GA->x(v) = points[i][0]; 84 | cout << "Here2\n"; 85 | GA->y(v) = points[i][1]; 86 | GA->width(v) = NODESIZE; 87 | GA->height(v) = NODESIZE; 88 | GA->shape(v) = ogdf::Shape::shEllipse; 89 | GA->fillColor(v) = Color("#FF0000"); // set colour 90 | GA->strokeColor(v) = Color("#FF0000"); 91 | 92 | node u; 93 | forall_nodes(u, *G){ 94 | edge e = G->newEdge(v,u); 95 | GA->bends(e); 96 | GA->arrowType(e) = ogdf::EdgeArrow::eaNone; 97 | GA->strokeColor(e) = Color("#FF0000"); 98 | } 99 | } 100 | 101 | } 102 | 103 | /* 104 | recursively grow the graph 105 | */ 106 | void genarateSierpinksiGraph(Graph *G, GraphAttributes *GA,float parent[3][2],int count) 107 | { 108 | if(count==0) 109 | { 110 | return; 111 | } 112 | 113 | node list[3]; 114 | for(int i=0;i<3;i++) 115 | { 116 | // create new node 117 | node v = G->newNode(); 118 | GA->x(v) = (parent[i][0] + parent[(i+1)%3][0])/2; 119 | GA->y(v) = (parent[i][1] + parent[(i+1)%3][1])/2; 120 | GA->width(v) = NODESIZE; 121 | GA->height(v) = NODESIZE; 122 | GA->shape(v) = ogdf::Shape::shEllipse; 123 | GA->fillColor(v) = Color("#FF0000"); 124 | 125 | list[i] = v; 126 | 127 | // create new edge 128 | for(int j=0;jnewEdge(v,list[j]); 131 | GA->bends(e); 132 | GA->arrowType(e) = ogdf::EdgeArrow::eaNone; 133 | GA->strokeColor(e) = Color("#FF0000"); 134 | } 135 | 136 | float child[3][2] = { 137 | {parent[i][0],parent[i][1]}, 138 | {(parent[i][0] + parent[(i+1)%3][0])/2,(parent[i][1] + parent[(i+1)%3][1])/2}, 139 | {(parent[i][0] + parent[(i+2)%3][0])/2,(parent[i][1] + parent[(i+2)%3][1])/2}}; 140 | 141 | // call for next iteration 142 | genarateSierpinksiGraph(G,GA,child,count-1); 143 | } 144 | } 145 | -------------------------------------------------------------------------------- /examples/snap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists 2 | cmake_minimum_required(VERSION 3.1) 3 | project (EXAMPLES) 4 | 5 | find_library(SNAP snap 6 | PATHS ../lib/snap/snap-core) 7 | #find_library(OGDF ogdf 8 | # PATHS ../lib/ogdf/_debug) 9 | 10 | ## OGDF Examples 11 | #FIND 12 | #FIND_PACKAGE( OGDF REQUIRED ) 13 | # set the path to the library folder 14 | link_directories(/usr/local/lib) 15 | 16 | # link the libraries to the executable 17 | 18 | #INCLUDE_DIRECTORIES( /usr/local/include/ogdf ) 19 | #INCLUDE_DIRECTORIES( /usr/local/include/coin ) 20 | 21 | INCLUDE_DIRECTORIES( ../lib/snap/snap-core ) 22 | 23 | ## c0x 24 | #include(CheckCXXCompilerFlag) 25 | #CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 26 | #CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 27 | #if(COMPILER_SUPPORTS_CXX11) 28 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 29 | #elseif(COMPILER_SUPPORTS_CXX0X) 30 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 31 | #else() 32 | #message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 33 | #endif() 34 | 35 | 36 | # #SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} -g" ) 37 | 38 | ## Build and Link Executables 39 | 40 | ADD_EXECUTABLE( directed_graph1 directed_graph1.cpp ) 41 | 42 | TARGET_LINK_LIBRARIES( directed_graph1 snap ) 43 | 44 | 45 | #ADD_EXECUTABLE( manual manual.cpp ) 46 | #ADD_EXECUTABLE( random random.cpp ) 47 | #ADD_EXECUTABLE( sierpinski sierpinski.cpp ) 48 | 49 | #TARGET_LINK_LIBRARIES( manual OGDF ) 50 | #TARGET_LINK_LIBRARIES( random OGDF ) 51 | #TARGET_LINK_LIBRARIES( sierpinski OGDF ) 52 | 53 | ## Boost Examples 54 | #FIND_PACKAGE( Boost 1.40 COMPONENTS program_options REQUIRED ) 55 | #INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} ) 56 | # 57 | ##ADD_EXECUTABLE( dgaTest directed_graph_test.cpp ) 58 | #ADD_EXECUTABLE( graphVizTest write_graphviz.cpp ) 59 | #ADD_EXECUTABLE( graphVizReadTest read_graphviz.cpp ) 60 | # 61 | #TARGET_LINK_LIBRARIES( graphVizTest ${Boost_LIBRARIES} ) 62 | ##TARGET_LINK_LIBRARIES( dgaTest ${Boost_LIBRARIES} ) 63 | #TARGET_LINK_LIBRARIES( graphVizReadTest ${Boost_LIBRARIES} ) 64 | 65 | # the compiler used for C files 66 | MESSAGE( STATUS "CMAKE_C_COMPILER: " ${CMAKE_C_COMPILER} ) 67 | 68 | # the compiler used for C++ files 69 | MESSAGE( STATUS "CMAKE_CXX_COMPILER: " ${CMAKE_CXX_COMPILER} ) 70 | 71 | # if the compiler is a variant of gcc, this should be set to 1 72 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCC: " ${CMAKE_COMPILER_IS_GNUCC} ) 73 | 74 | # if the compiler is a variant of g++, this should be set to 1 75 | MESSAGE( STATUS "CMAKE_COMPILER_IS_GNUCXX : " ${CMAKE_COMPILER_IS_GNUCXX} ) 76 | -------------------------------------------------------------------------------- /examples/snap/directed_graph1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main() 4 | { 5 | // create a graph 6 | PNGraph Graph = TNGraph::New(); 7 | Graph->AddNode(1); 8 | Graph->AddNode(5); 9 | Graph->AddNode(32); 10 | Graph->AddEdge(1,5); 11 | Graph->AddEdge(5,1); 12 | Graph->AddEdge(5,32); 13 | 14 | // print graph 15 | TFOut FOut = TFOut::New("directed_graph1.graph"); 16 | Graph->Save(FOut) 17 | 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /examples/solveDTSP_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Example showing how to build a path planner for a Dubins Traveling Salesperson 3 | * Problem (DTSP). The solution's tour, headings, and edges are printed. 4 | * 5 | * Copyright (C) 2014-2015 DubinsPathPlanner. 6 | * Created by David Goodman 7 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 8 | * For details see the LICENSE file distributed with DubinsPathPlanner. 9 | */ 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace std; 23 | 24 | /** 25 | * This example code solves an 8 waypoint triangle pattern with a turn radius 26 | * of 10 meters, and an initial heading of 0 radians (north). The initial position 27 | * is the first node in the loaded .gml graph. 28 | */ 29 | int main(int argc, char *argv[]) { 30 | 31 | // Read input arguments (this compiles into: build_dir/examples/) 32 | string inputFilename("../../data/triangle-8wp.gml"); 33 | double x = 0.0; 34 | double r = 10.0; 35 | 36 | // Build path planner and find a solution 37 | dpp::DubinsVehiclePathPlanner p; 38 | 39 | p.addWaypoints(inputFilename); 40 | p.initialHeading(0.0); 41 | p.turnRadius(1.0); 42 | p.returnToInitial(true); 43 | 44 | p.solve(); 45 | 46 | // Results. Must use graph pointer. 47 | ogdf::Graph *G = p.graphPtr(); 48 | ogdf::GraphAttributes GA = p.graphAttributes(); 49 | ogdf::List E = p.edges(); 50 | ogdf::List Tour = p.tour(); 51 | ogdf::NodeArray Headings = p.headings(); 52 | double cost = p.cost(); 53 | 54 | // Print headings and edge list 55 | ogdf::node u; 56 | std::cout << "Solved " << G->numberOfNodes() << " point tour with cost " << cost << "." << std::endl; 57 | std::cout << dpp::printHeadings(*G, GA, Headings); 58 | std::cout << dpp::printEdges(*G, GA, E); 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /examples/solveDtspSimple_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Example showing how to build a simple path planner that solves for an efficient 3 | * sequence of waypoints. This is the same interface used in the QGC implementation. 4 | * 5 | * Copyright (C) 2014-2015 DubinsPathPlanner. 6 | * Created by David Goodman 7 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 8 | * For details see the LICENSE file distributed with DubinsPathPlanner. 9 | */ 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #define TURN_RADIUS 10.0 // [m] 16 | #define INITIAL_HEADING_ANGLE 0.0 // [rad] 17 | 18 | // Waypoint scenario to solve. First point is initial point. 19 | const dpp::WaypointList waypoints { 20 | {0, 0}, 21 | {8.1*TURN_RADIUS, -10.3*TURN_RADIUS}, 22 | {5.1*TURN_RADIUS, 0.0}, 23 | {-2.0*TURN_RADIUS, -3.3*TURN_RADIUS} 24 | }; 25 | 26 | /** 27 | * This example code solves the waypoint pattern defined above with a turn radius 28 | * of 10 meters, and an initial heading of 0 radians (north). The initial position 29 | * is the first node in the loaded .gml graph. 30 | */ 31 | int main(int argc, char *argv[]) { 32 | 33 | // Build path planner and find a solution 34 | dpp::WaypointSequencePlanner p; 35 | p.initialHeading(INITIAL_HEADING_ANGLE); 36 | p.turnRadius(TURN_RADIUS); 37 | p.addWaypoints(waypoints); 38 | 39 | p.planWaypointSequence(); // synonymous with solve() 40 | 41 | // Get and print re-ordered list 42 | std::vector newSequenceList = p.newWaypointSequenceList(); 43 | double cost = p.cost(); 44 | 45 | std::cout << "Solved " << p.waypointCount() << " point tour with cost " << cost << "." << std::endl; 46 | std::cout << "New waypoint order: { "; 47 | bool first = true; 48 | for (const auto& i : newSequenceList) { 49 | if (!first) std::cout << ", "; 50 | std::cout << i; 51 | first = false; 52 | } 53 | std::cout << " }" << std::endl; 54 | 55 | return 0; 56 | } -------------------------------------------------------------------------------- /include/dpp/basic/Field.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_FIELD_H_ 2 | #define _DPP_FIELD_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace dpp { 12 | 13 | #define DPP_FIELD_DEFAULT_COVERAGE_WIDTH 1 14 | 15 | typedef ogdf::List PolyVertexList; 16 | typedef ogdf::ListIterator PolyVertexIterator; 17 | typedef ogdf::ListConstIterator PolyVertexConstIterator; 18 | 19 | class Field 20 | { 21 | public: 22 | /* 23 | Field() 24 | : m_poly(), 25 | m_coverageWidth(DPP_FIELD_DEFAULT_COVERAGE_WIDTH) 26 | { } 27 | */ 28 | 29 | Field(DPolygon poly, double coverageWidth = DPP_FIELD_DEFAULT_COVERAGE_WIDTH) 30 | : m_coverageWidth(coverageWidth), 31 | m_poly(poly) 32 | { 33 | m_poly.unify(); // delete duplicates 34 | computeBoundingBox(); 35 | } 36 | 37 | Field(PolyVertexList vertices, double coverageWidth = DPP_FIELD_DEFAULT_COVERAGE_WIDTH); 38 | 39 | ~Field() 40 | { } 41 | 42 | double coverageWidth(void) const { 43 | return m_coverageWidth; 44 | } 45 | 46 | void coverageWidth(double e) { 47 | m_coverageWidth = e; 48 | } 49 | 50 | bool isCcw(void); 51 | 52 | bool isConvex(void) const; 53 | 54 | PolyVertexIterator getVertexWithMinX(void) { 55 | return m_minXVertex; 56 | } 57 | 58 | PolyVertexIterator getVertexWithMaxX(void) { 59 | return m_maxXVertex; 60 | } 61 | 62 | PolyVertexIterator getVertexWithMinY(void) { 63 | return m_minYVertex; 64 | } 65 | 66 | PolyVertexIterator getVertexWithMaxY(void) { 67 | return m_maxYVertex; 68 | } 69 | /* 70 | DPolygon *polygon(void) { 71 | return &m_poly; 72 | } 73 | */ 74 | const DPolygon *polygon(void) const { 75 | return const_cast(&m_poly); 76 | } 77 | 78 | int findMinimumWidth(double &width, double &angle) const; 79 | 80 | int addNodesFromGrid(ogdf::Graph &G, ogdf::GraphAttributes &GA) const; 81 | 82 | int generateFieldTracks(FieldTrackList &tracks) const; 83 | 84 | friend ostream& operator<<(ostream& os, const Field& f); 85 | 86 | private: 87 | DPolygon m_poly; 88 | double m_coverageWidth; 89 | PolyVertexIterator m_minXVertex, m_minYVertex, m_maxXVertex, m_maxYVertex; 90 | void computeBoundingBox(void); 91 | 92 | }; 93 | 94 | inline ostream& operator<<(ostream& os, const Field& f) { 95 | DPP_ASSERT(f.polygon()->size() > 0); 96 | PolyVertexConstIterator iter; 97 | for ( iter = f.polygon()->begin(); iter != f.polygon()->end(); iter++ ) { 98 | os << " " << *iter << std::endl; 99 | } 100 | return os; 101 | } 102 | 103 | /// Sweep-line used for generating field tracks. Field must be convex. 104 | class FieldTrackSweepLine : public Line2d 105 | { 106 | public: 107 | FieldTrackSweepLine(DSegment s) 108 | : Line2d(s) 109 | { } 110 | 111 | bool intersectingTrack(const Field *field, FieldTrack &track) const; 112 | 113 | private: 114 | }; 115 | 116 | 117 | // Non-member functions related to Field 118 | // FIXME move to util? 119 | inline Vector2d segmentToVector(DSegment s) { 120 | return Vector2d(s.dx(), s.dy()); 121 | } 122 | 123 | /** 124 | * Find the distance between the point p and the caliper line formed by 125 | * the caliper's unit vector and incident point. 126 | */ 127 | inline double distanceToCaliper(DPoint p, DPoint calPoint, Vector2d calVector) { 128 | DPoint calPoint2(calPoint.m_x + calVector.x(), calPoint.m_y + calVector.y()); 129 | // perpendicular vector to caliper 130 | Vector2d v(calPoint2.m_y - calPoint.m_y, -(calPoint2.m_x - calPoint.m_x)); 131 | v = v.normalized(); 132 | // vector between p and caliper 133 | Vector2d r(calPoint.m_x - p.m_x, calPoint2.m_y - p.m_y); 134 | 135 | return fabs(v.dot(r)); 136 | } 137 | 138 | bool findPolySegmentWithAngle(double angle, const DPolygon *poly, DSegment &seg, bool dir=true); 139 | 140 | } // namespace dpp 141 | 142 | #endif // _DPP_FIELD_H_ 143 | -------------------------------------------------------------------------------- /include/dpp/basic/FieldTrack.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_FIELD_TRACK_H_ 2 | #define _DPP_FIELD_TRACK_H_ 3 | 4 | #include 5 | #include 6 | 7 | using ogdf::DPoint; 8 | using ogdf::DPolygon; 9 | using ogdf::DSegment; 10 | 11 | namespace dpp { 12 | 13 | class FieldTrack : private DLine 14 | { 15 | public: 16 | using DLine::dx; 17 | using DLine::dy; 18 | using DLine::isHorizontal; 19 | using DLine::isVertical; 20 | using DLine::length; 21 | using DLine::slope; 22 | using DLine::start; 23 | using DLine::end; 24 | using DLine::contains; 25 | 26 | FieldTrack(void) 27 | : m_reverse(false) 28 | { } 29 | 30 | FieldTrack(DSegment s) 31 | : DLine(s.start(), s.end()) 32 | { } 33 | 34 | FieldTrack(DPoint start, DPoint end) 35 | : DLine(start, end) 36 | { } 37 | 38 | ~FieldTrack(void) 39 | { } 40 | 41 | const DPoint& start(void) const { 42 | if (!m_reverse) { 43 | return DLine::start(); 44 | } 45 | else { 46 | return DLine::end(); 47 | } 48 | } 49 | 50 | const DPoint& end(void) const { 51 | if (!m_reverse) { 52 | return DLine::end(); 53 | } 54 | else { 55 | return DLine::start(); 56 | } 57 | } 58 | 59 | // Equality only compares to see if endpoints are identical. start/end order donesn't matter 60 | bool operator== (const FieldTrack &track) const { 61 | return (start() == track.start() && end() == track.end()) 62 | || (end() == track.start() && start() == track.end()); 63 | } 64 | 65 | // Not equals 66 | bool operator!= (const FieldTrack &track) const { 67 | return !(*this == track); 68 | } 69 | 70 | /* 71 | DPoint endNode1(void) { 72 | return m_endNode1; 73 | } 74 | 75 | DPoint endNode2(void) { 76 | return m_endNode2; 77 | } 78 | */ 79 | 80 | /** 81 | * Returns the angle of the track line from start to end between [0, 2*pi). 82 | */ 83 | double angle(void) const { 84 | DLine line(start(), end()); 85 | if (m_reverse) { 86 | line = DLine(end(), start()); 87 | } 88 | return angleOfLine(line); 89 | } 90 | 91 | /** 92 | * Returns the angle of the track line from start to end between [0, 2*pi). 93 | */ 94 | double angle(bool reverse) const { 95 | DLine line(start(), end()); 96 | if (reverse) { 97 | line = DLine(end(), start()); 98 | } 99 | return angleOfLine(line); 100 | } 101 | /* 102 | void addEndsToGraph(ogdf::Graph &G, ogdf::GraphAttributes &GA) { 103 | // TODO move to .cpp, and add edges as well 104 | DPP_ASSERT(start() != end()); // FIXME add check for node NOT in graph? 105 | m_endNode1 = G.newNode(); 106 | GA.x(m_endNode1) = start().m_x; 107 | GA.y(m_endNode1) = start().m_y; 108 | m_endNode2 = G.newNode(); 109 | GA.x(m_endNode2) = end().m_x; 110 | GA.y(m_endNode2) = end().m_y; 111 | } 112 | */ 113 | bool reverse(void) const { 114 | return m_reverse; 115 | } 116 | 117 | void reverse(bool rev) { 118 | m_reverse = rev; 119 | } 120 | 121 | friend ostream& operator<<(ostream& os, const FieldTrack& t); 122 | 123 | private: 124 | //ogdf::NodeSet m_endPointNodes; 125 | //ogdf::node m_endNode1, m_endNode2; 126 | bool m_reverse; 127 | }; 128 | 129 | inline ostream& operator<<(ostream& os, const FieldTrack& t) { 130 | return os << t.start() << ((!t.reverse())? "->" : "<-") << t.end(); 131 | } 132 | 133 | class FieldTrackList : public ogdf::List 134 | { 135 | public: 136 | // inherit constructors 137 | using List::List; 138 | 139 | int addNodesToGraph(ogdf::Graph &G, ogdf::GraphAttributes &GA, 140 | double distance, ogdf::List &Tour, 141 | ogdf::NodeArray &Headings) const; 142 | 143 | private: 144 | }; 145 | 146 | typedef ogdf::ListIterator FieldTrackListIterator; 147 | typedef ogdf::ListConstIterator FieldTrackListConstIterator; 148 | 149 | } // namespace dpp 150 | 151 | #endif // _DPP_FIELD_TRACK_H_ 152 | -------------------------------------------------------------------------------- /include/dpp/basic/FileIO.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_FILE_IO_H_ 2 | #define _DPP_FILE_IO_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #define TSP_FILE_EXTENSION ".tsp" 9 | #define PAR_FILE_EXTENSION ".par" 10 | 11 | namespace dpp { 12 | 13 | /** 14 | * Class for TSPlib files. 15 | */ 16 | class TspFile { 17 | public: 18 | enum ProblemType { ETSP, ATSP, HCP }; 19 | static const char *ProblemTypeText[]; 20 | }; 21 | 22 | /** 23 | * Class for GML files. 24 | */ 25 | class GmlFile { 26 | public: 27 | enum Type { GRAPH, DPOLYGON }; 28 | static const char *TypeText[]; 29 | }; 30 | 31 | /** 32 | * For read/write of a DLM (CSV) file. Reads List of DPoints. 33 | class PolygonFile { 34 | public: 35 | enum Type {} 36 | }; // class DlmFile 37 | */ 38 | 39 | /* TODO create classes 40 | - PARFile 41 | - TSPFile abstract 42 | - ATSPFile : public TSPFile 43 | - ETSPFile : public TSPFile 44 | */ 45 | 46 | /* 47 | class TSPFile { 48 | public: 49 | int writeFile(); 50 | 51 | }; 52 | */ 53 | 54 | // Static function to eventually add into classes 55 | int writeAtspFile(std::string filename, std::string name, std::string comment, 56 | ogdf::Graph &G, NodeMatrix &A); 57 | 58 | int writeEtspFile(std::string filename, std::string name, std::string comment, 59 | ogdf::Graph &G, ogdf::GraphAttributes &GA); 60 | 61 | int writeParFile(std::string filename, std::string tspFilename, std::string outputFilename, int runs=10); 62 | 63 | int readTspTourFile(std::string filename, ogdf::Graph &G, ogdf::GraphAttributes &GA, 64 | ogdf::List &tour, bool returnToInitial=true); 65 | 66 | 67 | int readPointsFromGmlFile(std::string filename, ogdf::List &Points); 68 | 69 | int readGraphFromGmlFile(std::string filename, ogdf::Graph &G, ogdf::GraphAttributes &GA); 70 | 71 | int readPolygonFromGmlFile(std::string filename, ogdf::DPolygon &poly); 72 | 73 | } // namespace dpp 74 | 75 | #endif // _DPP_FILE_IO_H_ 76 | -------------------------------------------------------------------------------- /include/dpp/basic/Logger.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_LOGGER_H_ 2 | #define _DPP_LOGGER_H_ 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #define DPP_LOGGER_LEVEL_DEBUG (dpp::Logger::Level::LL_DEBUG) 11 | #define DPP_LOGGER_LEVEL_INFO (dpp::Logger::Level::LL_INFO) 12 | #define DPP_LOGGER_LEVEL_WARN (dpp::Logger::Level::LL_WARN) 13 | #define DPP_LOGGER_LEVEL_ERROR (dpp::Logger::Level::LL_ERROR) 14 | 15 | #define DPP_LOGGER_VERBOSE_0 (dpp::Logger::Verbosity::LV_VERBOSE_0) 16 | #define DPP_LOGGER_VERBOSE_1 (dpp::Logger::Verbosity::LV_VERBOSE_1) 17 | #define DPP_LOGGER_VERBOSE_2 (dpp::Logger::Verbosity::LV_VERBOSE_2) 18 | #define DPP_LOGGER_VERBOSE_3 (dpp::Logger::Verbosity::LV_VERBOSE_3) 19 | 20 | 21 | #define DPP_LOGGER_DEFAULT_VERBOSE DPP_LOGGER_VERBOSE_0 22 | 23 | // Set logger level to debug if compiled in debug mode, otherwise info 24 | #ifdef DPP_DEBUG 25 | #define DPP_LOGGER_DEFAULT_LEVEL DPP_LOGGER_LEVEL_DEBUG 26 | #else 27 | #define DPP_LOGGER_DEFAULT_LEVEL DPP_LOGGER_LEVEL_INFO 28 | #endif 29 | 30 | namespace dpp { 31 | 32 | class Logger { 33 | public: 34 | enum Level { LL_DEBUG, LL_INFO, LL_WARN, LL_ERROR }; 35 | enum Verbosity { LV_VERBOSE_0 = 0, LV_VERBOSE_1, LV_VERBOSE_2, LV_VERBOSE_3 }; // ascending verbosity 36 | 37 | ~Logger(); 38 | 39 | static Logger* Instance(void); 40 | 41 | static void initializeLogger(std::ostream &ostream); 42 | 43 | static void initializeLogger(std::string logFilename); 44 | 45 | static std::ostream & logOut(Logger::Level l = Logger::Level::LL_INFO, 46 | Logger::Verbosity v = Logger::Verbosity::LV_VERBOSE_0); 47 | 48 | static std::ostream & logDebug(Logger::Verbosity v = DPP_LOGGER_DEFAULT_VERBOSE) { 49 | return logOut(Logger::Level::LL_DEBUG, v); 50 | } 51 | 52 | static std::ostream & logInfo(Logger::Verbosity v = DPP_LOGGER_DEFAULT_VERBOSE) { 53 | return logOut(Logger::Level::LL_INFO, v); 54 | } 55 | 56 | static std::ostream & logWarn(Logger::Verbosity v = DPP_LOGGER_DEFAULT_VERBOSE) { 57 | return logOut(Logger::Level::LL_WARN, v); 58 | } 59 | 60 | static std::ostream & logError(Logger::Verbosity v = DPP_LOGGER_DEFAULT_VERBOSE) { 61 | return logOut(Logger::Level::LL_ERROR, v); 62 | } 63 | 64 | Level level(void) { 65 | return m_level; 66 | } 67 | 68 | void level(Level l) { 69 | m_level = l; 70 | } 71 | 72 | Verbosity verbose(void) { 73 | return m_verbosity; 74 | } 75 | 76 | void verbose(int i) { 77 | DPP_ASSERT(i >= (dpp::Logger::Verbosity::LV_VERBOSE_0)); 78 | m_verbosity = static_cast(i); 79 | } 80 | 81 | bool isUsingFile(void) { 82 | return m_useFile; 83 | } 84 | 85 | private: 86 | // Functions 87 | // TODO add dual stream (cout and file) 88 | Logger() 89 | : m_logger(), 90 | m_logFile(), 91 | m_useFile(false), 92 | m_verbosity(DPP_LOGGER_DEFAULT_VERBOSE), 93 | m_level(DPP_LOGGER_DEFAULT_LEVEL) 94 | { } 95 | 96 | Logger(Logger const&) // copy constructor is private for singleton class 97 | { } 98 | 99 | Logger& operator=(Logger const&) // assignment is private ------"------ 100 | { 101 | return *this; 102 | } 103 | 104 | // Attributes 105 | static Logger* m_pInstance; 106 | static std::ostream nullStream; 107 | ogdf::Logger m_logger; 108 | std::ofstream m_logFile; 109 | Level m_level; 110 | Verbosity m_verbosity; 111 | bool m_useFile; 112 | }; 113 | 114 | 115 | } // namespace dpp 116 | 117 | #endif // _DPP_LOGGER_H_ -------------------------------------------------------------------------------- /include/dpp/basic/NodeMatrix.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_NODE_MATRIX_H_ 2 | #define _DPP_NODE_MATRIX_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace dpp { 10 | 11 | template 12 | class NodeMatrix 13 | { 14 | public: 15 | NodeMatrix() { 16 | m_nodes = 0; 17 | } 18 | 19 | NodeMatrix( ogdf::Graph &G ) { 20 | allocate( G ); 21 | } 22 | 23 | ~NodeMatrix() { 24 | deallocate(); 25 | } 26 | 27 | ogdf::NodeArray & operator[](const ogdf::node &v) { 28 | return (*m_data)[v]; 29 | //return ogdf::NodeArray::operator [](v->index()); 30 | } 31 | 32 | T get( const ogdf::node &i, const ogdf::node &j ) { 33 | return (*m_data)[i][j]; 34 | } 35 | 36 | void set( const T& t, const ogdf::node &i, const ogdf::node &j) { 37 | (*m_data)[i][j] = t; 38 | } 39 | 40 | void setAll( const T& t) { 41 | if (m_graph == nullptr) 42 | return; 43 | 44 | ogdf::node i, j; 45 | forall_nodes(i, *m_graph) { 46 | forall_nodes(j, *m_graph) { 47 | (*m_data)[i][j] = t; 48 | } 49 | } 50 | } 51 | 52 | int numberOfNodes() { 53 | return m_nodes; 54 | } 55 | 56 | const ogdf::Graph * graphOf() { 57 | return m_graph; 58 | } 59 | 60 | private: 61 | void allocate( ogdf::Graph &G ) { 62 | m_nodes = G.numberOfNodes(); 63 | m_graph = &G; 64 | 65 | m_data = new ogdf::NodeArray>(G); 66 | 67 | ogdf::node i; 68 | forall_nodes(i, G) { 69 | (*m_data)[i] = ogdf::NodeArray(G); 70 | } 71 | 72 | setAll(0.0); 73 | } 74 | 75 | void deallocate() { 76 | delete m_data; 77 | m_nodes = 0; 78 | } 79 | 80 | // Attributes 81 | int m_nodes; 82 | ogdf::NodeArray> *m_data; 83 | const ogdf::Graph *m_graph; 84 | }; 85 | 86 | } // namespace dpp 87 | 88 | #endif // _DPP_NODE_MATRIX_H_ 89 | 90 | -------------------------------------------------------------------------------- /include/dpp/basic/Path.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_PATH_H_ 2 | #define _DPP_PATH_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | #define DPP_MAX_EDGE_COST 999999.0 // FIXME scale this based on edge costs 20 | 21 | namespace dpp { 22 | 23 | 24 | // Public Prototypes 25 | double dubinsPathLength(VehicleConfiguration &Cs, VehicleConfiguration &Ce, 26 | double turnRadius); 27 | 28 | double dubinsTourCost(ogdf::Graph &G, ogdf::GraphAttributes &GA, 29 | ogdf::List &Tour, ogdf::NodeArray &Headings, 30 | double turnRadius, bool returnCost = false); 31 | 32 | double createDubinsTourEdges(ogdf::Graph &G, ogdf::GraphAttributes &GA, 33 | ogdf::List &Tour, ogdf::NodeArray &Headings, 34 | double turnRadius, ogdf::List &Edges, bool returnEdge = false); 35 | 36 | void buildDubinsAdjacencyMatrix(ogdf::Graph &G, ogdf::GraphAttributes &GA, 37 | NodeMatrix &A, ogdf::NodeArray &Headings, double turnRadius); 38 | 39 | } // namespace dpp 40 | 41 | #endif // _DPP_PATH_H_ 42 | -------------------------------------------------------------------------------- /include/dpp/basic/VehicleConfiguration.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_VEHICLE_CONFIGURATION_H_ 2 | #define _DPP_VEHICLE_CONFIGURATION_H_ 3 | 4 | #include 5 | #include // std::out_of_range 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | using Eigen::Vector3d; 15 | using Eigen::Vector2d; 16 | 17 | namespace dpp { 18 | 19 | class VehicleConfiguration 20 | { 21 | public: 22 | // Public attributes 23 | ogdf::DPoint m_position; 24 | double m_heading; // [rad[ 25 | 26 | // Public methods 27 | VehicleConfiguration() { 28 | m_position.m_x = 0.0; 29 | m_position.m_y = 0.0; 30 | m_heading = 0.0; 31 | } 32 | 33 | VehicleConfiguration(double x, double y, double heading=0.0) { 34 | m_position.m_x = x; 35 | m_position.m_y = y; 36 | this->heading(heading); // for debug assertion on bounds 37 | } 38 | 39 | VehicleConfiguration(const VehicleConfiguration &C) { 40 | m_position = C.m_position; 41 | m_heading = C.m_heading; 42 | } 43 | 44 | // Operator overloading 45 | /// Copy constructor 46 | VehicleConfiguration & operator=(const VehicleConfiguration &C) { 47 | if (C != *this) { 48 | m_position = C.m_position; 49 | m_heading = C.m_heading; 50 | } 51 | return *this; 52 | } 53 | 54 | bool operator==(const VehicleConfiguration &C) const { 55 | return (m_position == C.m_position && 56 | m_heading == C.m_heading); 57 | } 58 | 59 | bool operator!=(VehicleConfiguration &C) const { 60 | return !(*this == C); 61 | } 62 | 63 | friend std::ostream & operator<<(std::ostream & stream, VehicleConfiguration const & v); 64 | 65 | // Accessors 66 | double x() { 67 | return m_position.m_x; 68 | } 69 | 70 | double y() { 71 | return m_position.m_y; 72 | } 73 | 74 | double heading() { 75 | return m_heading; 76 | } 77 | 78 | ogdf::DPoint& position(void) { 79 | return m_position; 80 | } 81 | 82 | // Modifiers 83 | void position(double x, double y) { 84 | m_position.m_x = x; 85 | m_position.m_y = y; 86 | } 87 | 88 | void heading(double x) { 89 | DPP_ASSERT(x >= 0 && x < 2.0*M_PI); 90 | m_heading = x; 91 | } 92 | 93 | void x(double x) { 94 | m_position.m_x = x; 95 | } 96 | 97 | void y(double y) { 98 | m_position.m_y = y; 99 | } 100 | 101 | // Methods 102 | void asArray(double **q) { 103 | *q = new double[3]; 104 | (*q)[0] = m_position.m_x; 105 | (*q)[1] = m_position.m_y; 106 | (*q)[2] = m_heading; 107 | } 108 | 109 | Vector2d asVector() { 110 | return Vector2d(m_position.m_x, m_position.m_y); 111 | } 112 | 113 | double euclideanDistance(VehicleConfiguration &C) { 114 | return m_position.distance(C.m_position); 115 | } 116 | }; 117 | 118 | inline std::ostream & operator<<(std::ostream & stream, const VehicleConfiguration & v) { 119 | stream << "(" << v.m_position.m_x << ", " << v.m_position.m_y << ", " << v.m_heading << ")"; 120 | return stream; 121 | } 122 | 123 | } // namespace dpp 124 | 125 | #endif // _DPP_VEHICLE_CONFIGURATION_H_ 126 | -------------------------------------------------------------------------------- /include/dpp/basic/basic.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_BASIC_H_ 2 | #define _DPP_BASIC_H_ 3 | #include 4 | 5 | #ifdef DPP_DEBUG 6 | #include 7 | #define DPP_ASSERT(expr) assert(expr); 8 | //#define OGDF_ASSERT_IF(minLevel,expr) \ 9 | // if (int(ogdf::debugLevel) >= int(minLevel)) { assert(expr); } else { } 10 | //#define DPP_SET_DEBUG_LEVEL(level) ogdf::debugLevel = level; 11 | #else 12 | #define DPP_ASSERT(expr) 13 | //#define DPP_ASSERT_IF(minLevel,expr) 14 | //#define DPP_SET_DEBUG_LEVEL(level) 15 | #endif 16 | 17 | #define FAILURE (-1) 18 | #define SUCCESS (0) 19 | 20 | #define DPP_GRAPH_ATTRIBUTES ( \ 21 | ogdf::GraphAttributes::nodeGraphics | \ 22 | ogdf::GraphAttributes::edgeGraphics | \ 23 | ogdf::GraphAttributes::nodeLabel | \ 24 | ogdf::GraphAttributes::edgeStyle | \ 25 | ogdf::GraphAttributes::edgeDoubleWeight | \ 26 | ogdf::GraphAttributes::nodeStyle | \ 27 | ogdf::GraphAttributes::nodeTemplate | \ 28 | ogdf::GraphAttributes::nodeId) 29 | 30 | namespace dpp { 31 | 32 | } // namespace dpp 33 | 34 | #endif // _DPP_BASIC_H_ -------------------------------------------------------------------------------- /include/dpp/planalg/Algorithm.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_ALGORITHM_H_ 2 | #define _DPP_ALGORITHM_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using ogdf::node; 19 | using ogdf::edge; 20 | using ogdf::Graph; 21 | using ogdf::GraphAttributes; 22 | using ogdf::List; 23 | using ogdf::ListIterator; 24 | using ogdf::NodeArray; 25 | 26 | namespace dpp { 27 | 28 | /* 29 | * Base class for solving. 30 | */ 31 | class Algorithm { 32 | public: 33 | enum Type {DTSP, CPP}; 34 | 35 | Algorithm(std::string name, Type t) 36 | : m_name(name), 37 | m_type(t) 38 | { } 39 | 40 | virtual ~Algorithm() = 0; 41 | 42 | static int runLKHSolver(std::string parFilename); 43 | 44 | const Type type(void) { 45 | return m_type; 46 | } 47 | 48 | const std::string typeText(void) { 49 | return TypeText[m_type]; 50 | } 51 | 52 | const std::string name(void) { 53 | return m_name; 54 | } 55 | 56 | protected: 57 | static const char *TypeText[]; 58 | const std::string m_name; 59 | const Type m_type; 60 | }; 61 | 62 | 63 | /* 64 | * Dubins Traveling Salesperson Problem algorithm. 65 | */ 66 | class AlgorithmDtsp : public Algorithm { 67 | public: 68 | AlgorithmDtsp(std::string name) 69 | : Algorithm(name, Algorithm::Type::DTSP) 70 | { } 71 | 72 | ~AlgorithmDtsp() 73 | { } 74 | 75 | virtual int run(Graph &G, GraphAttributes &GA, double x, double r, 76 | List &Tour, List &Edges, NodeArray &Headings, double &cost, 77 | bool returnToInitial=true) = 0; 78 | }; 79 | 80 | /* 81 | * Coverage Path Planning algorithm. 82 | */ 83 | class AlgorithmCpp : public Algorithm { 84 | public: 85 | AlgorithmCpp(std::string name) 86 | : Algorithm(name, Algorithm::Type::CPP) 87 | { } 88 | 89 | ~AlgorithmCpp() { 90 | } 91 | 92 | virtual int run(const Field &field, VehicleConfiguration C, double r, 93 | bool returnToInitial, Graph &G, GraphAttributes &GA, List &Tour, 94 | List &Edges, NodeArray &Headings, double &cost) = 0; 95 | }; 96 | 97 | 98 | } // namespace dpp 99 | 100 | #endif // _DPP_ALGORITHM_H_ -------------------------------------------------------------------------------- /include/dpp/planalg/AlternatingDtsp.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_ALGORITHM_ALTERNATING_DTSP_H_ 2 | #define _DPP_ALGORITHM_ALTERNATING_DTSP_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace dpp { 9 | /* 10 | * Alternating algorithm for the Dtsp. 11 | */ 12 | class AlternatingDtsp : public AlgorithmDtsp { 13 | public: 14 | AlternatingDtsp() 15 | : AlgorithmDtsp(std::string("Alternating")) 16 | { } 17 | 18 | ~AlternatingDtsp() 19 | { } 20 | 21 | int run(Graph &G, GraphAttributes &GA, double x, double r, List &Tour, 22 | List &Edges, NodeArray &Headings, double &cost, 23 | bool returnToInitial=true); 24 | }; 25 | 26 | } // namespace dpp 27 | #endif // _DPP_ALGORITHM_ALTERNATING_DTSP_H_ -------------------------------------------------------------------------------- /include/dpp/planalg/BoustrophedonCpp.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_BOUTSTROPHEDON_CPP_H_ 2 | #define _DPP_BOUTSTROPHEDON_CPP_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace dpp { 10 | 11 | /* 12 | * Boustrophedon algorithm for CPP. 13 | */ 14 | class BoustrophedonCpp : public AlgorithmCpp { 15 | public: 16 | BoustrophedonCpp() 17 | : AlgorithmCpp(std::string("Boustrophedon")) 18 | { } 19 | 20 | ~BoustrophedonCpp() 21 | { } 22 | 23 | int run(const Field &field, VehicleConfiguration C, double turnRadius, 24 | bool returnToInitial, Graph &G, GraphAttributes &GA, List &Tour, 25 | List &Edges, NodeArray &Headings, double &cost); 26 | 27 | }; 28 | 29 | } // namespace dpp 30 | 31 | #endif // _DPP_BOUTSTROPHEDON_CPP_H_ -------------------------------------------------------------------------------- /include/dpp/planalg/NearestNeighborDtsp.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_ALGORITHM_NEAREST_NEIGHBOR_DTSP_H_ 2 | #define _DPP_ALGORITHM_NEAREST_NEIGHBOR_DTSP_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace dpp { 9 | /* 10 | * Nearest neighbor algorithm for the Dtsp. 11 | */ 12 | class NearestNeighborDtsp : public AlgorithmDtsp { 13 | public: 14 | NearestNeighborDtsp() 15 | : AlgorithmDtsp(std::string("NearestNeighbor")) 16 | { } 17 | 18 | ~NearestNeighborDtsp() 19 | { } 20 | 21 | int run(Graph &G, GraphAttributes &GA, double x, double r, List &Tour, 22 | List &Edges, NodeArray &Headings, double &cost, 23 | bool returnToInitial=true); 24 | }; 25 | 26 | } // namespace dpp 27 | #endif // _DPP_ALGORITHM_NEAREST_NEIGHBOR_DTSP_H_ -------------------------------------------------------------------------------- /include/dpp/planalg/RandomizedDtsp.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_RANDOMIZED_DTSP_H_ 2 | #define _DPP_RANDOMIZED_DTSP_H_ 3 | 4 | #include 5 | 6 | namespace dpp { 7 | 8 | /* 9 | * Randomized heading algorithm for the Dtsp. 10 | */ 11 | class RandomizedDtsp : public AlgorithmDtsp { 12 | public: 13 | RandomizedDtsp() 14 | : AlgorithmDtsp(std::string("Randomized")) 15 | { } 16 | 17 | ~RandomizedDtsp() 18 | { } 19 | 20 | int run(Graph &G, GraphAttributes &GA, double x, double r, List &Tour, 21 | List &Edges, NodeArray &Headings, double &cost, 22 | bool returnToInitial=true); 23 | 24 | }; 25 | 26 | } // namespace dpp 27 | #endif // _DPP_RANDOMIZED_DTSP_H_ -------------------------------------------------------------------------------- /include/dpp/planner/CoverageWaypointPlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_COVERAGE_WAYPOINT_PLANNER_H_ 2 | #define _DPP_COVERAGE_WAYPOINT_PLANNER_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace dpp { 8 | 9 | typedef Waypoint Vertex; 10 | typedef WaypointList VertexList; 11 | 12 | class CoverageWaypointPlanner : private DubinsSensorPathPlanner { 13 | public: 14 | using DubinsSensorPathPlanner::initialConfiguration; 15 | using DubinsSensorPathPlanner::sensorWidth; 16 | using DubinsPathPlanner::turnRadius; 17 | using DubinsSensorPathPlanner::algorithm; 18 | using PathPlanner::algorithmName; 19 | using PathPlanner::waypointCount; 20 | using PathPlanner::cost; 21 | using PathPlanner::haveSolution; 22 | 23 | CoverageWaypointPlanner(double turnRadius = 1.0, double sensorWidth = 0.0) 24 | : DubinsSensorPathPlanner(turnRadius, sensorWidth) 25 | { } 26 | 27 | 28 | ~CoverageWaypointPlanner() 29 | { } 30 | 31 | bool solve(void) { 32 | return planCoverageWaypoints(); 33 | } 34 | 35 | bool planCoverageWaypoints(void); 36 | 37 | WaypointList waypointList(void) { 38 | DPP_ASSERT(haveSolution()); 39 | return m_waypointList; 40 | } 41 | 42 | // FIXME move some of these functions up to the DubinsSensorPathPlanner 43 | void addPolygonVertex(Vertex v); 44 | 45 | int addPolygonVertices(VertexList list); 46 | 47 | int vertexCount(void) { 48 | return m_polygon.size(); 49 | } 50 | 51 | private: 52 | WaypointList m_waypointList; 53 | }; 54 | 55 | } // dpp 56 | 57 | #endif // _DPP_COVERAGE_WAYPOINT_PLANNER_H_ -------------------------------------------------------------------------------- /include/dpp/planner/DubinsSensorPathPlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_DUBINS_SENSOR_PATH_PLANNER_H_ 2 | #define _DPP_DUBINS_SENSOR_PATH_PLANNER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using ogdf::DPoint; 10 | using ogdf::DPolygon; 11 | 12 | namespace dpp { 13 | 14 | class DubinsSensorPathPlanner : public DubinsPathPlanner { 15 | public: 16 | enum CppPlanningAlgorithm { BOUSTROPHEDON }; 17 | 18 | DubinsSensorPathPlanner(double turnRadius=1.0, double sensorWidth=1.0) 19 | : m_sensorWidth(sensorWidth), 20 | DubinsPathPlanner(turnRadius, new BoustrophedonCpp) 21 | { } 22 | 23 | ~DubinsSensorPathPlanner() 24 | { } 25 | 26 | void polygon(std::string gmlFilename); 27 | 28 | void polygon(DPolygon polygon); 29 | 30 | void polygon(List points); 31 | 32 | DPolygon& polygon(void) { 33 | return m_polygon; 34 | } 35 | 36 | /// Set the sensor coverage width. 37 | void sensorWidth(double e) { 38 | m_sensorWidth = e; 39 | } 40 | 41 | /// Get the sensor coverage width. 42 | double sensorWidth(void) { 43 | return m_sensorWidth; 44 | } 45 | 46 | /// Get the initial vehicle configuration 47 | VehicleConfiguration initialConfiguration(void) { 48 | return m_initialConfig; 49 | } 50 | 51 | /// Set the initial vehicle configuration. 52 | void initialConfiguration(VehicleConfiguration C) { 53 | m_initialConfig = C; 54 | m_initialHeading = m_initialConfig.heading(); 55 | } 56 | 57 | /// Set the initial vehicle configuration. 58 | void initialConfiguration(double x, double y, double heading) { 59 | VehicleConfiguration C(x, y, heading); 60 | initialConfiguration(C); 61 | } 62 | 63 | 64 | /// Override to update heading of vehicle config. 65 | void initialHeading(double x) { 66 | m_initialConfig.heading(x); 67 | m_initialHeading = x; 68 | } 69 | 70 | void algorithm(CppPlanningAlgorithm algId); 71 | 72 | bool solveAsDtsp(DtspPlanningAlgorithm algId = DtspPlanningAlgorithm::ALTERNATING); 73 | 74 | bool solve(void); 75 | 76 | protected: 77 | DPolygon m_polygon; 78 | double m_sensorWidth; 79 | VehicleConfiguration m_initialConfig; 80 | 81 | }; // DubinsSensorPathPlanner 82 | 83 | typedef DubinsSensorPathPlanner::CppPlanningAlgorithm CppPlanningAlgorithm; 84 | 85 | } // namespace dpp 86 | 87 | #endif // _DPP_DUBINS_SENSOR_PATH_PLANNER_H_ -------------------------------------------------------------------------------- /include/dpp/planner/DubinsVehiclePathPlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_DUBINS_VEHICLE_PATH_PLANNER_H_ 2 | #define _DPP_DUBINS_VEHICLE_PATH_PLANNER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace dpp { 10 | 11 | class DubinsVehiclePathPlanner : public DubinsPathPlanner { 12 | public: 13 | enum DtspPlanningAlgorithm {NEAREST_NEIGHBOR, ALTERNATING, RANDOMIZED}; 14 | 15 | DubinsVehiclePathPlanner(double turnRadius = 1.0, 16 | Algorithm *alg = new AlternatingDtsp) 17 | : DubinsPathPlanner(turnRadius, alg) 18 | { } 19 | 20 | ~DubinsVehiclePathPlanner() 21 | { } 22 | 23 | void addWaypoints(std::string gmlFilename); 24 | 25 | void addWaypoints(ogdf::Graph &G, ogdf::GraphAttributes &GA); 26 | 27 | void algorithm(DtspPlanningAlgorithm algId); 28 | 29 | bool solve(void); 30 | 31 | private: 32 | 33 | }; 34 | 35 | typedef DubinsVehiclePathPlanner::DtspPlanningAlgorithm DtspPlanningAlgorithm; 36 | 37 | 38 | } // namespace dpp 39 | 40 | 41 | #endif // _DPP_DUBINS_VEHICLE_PATH_PLANNER_H_ -------------------------------------------------------------------------------- /include/dpp/planner/PathPlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_PATH_PLANNER_H_ 2 | #define _DPP_PATH_PLANNER_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using ogdf::EdgeArray; 14 | 15 | namespace dpp { 16 | /* 17 | * Base class for a path planner. 18 | */ 19 | class PathPlanner { 20 | public: 21 | PathPlanner(Algorithm *alg) 22 | : m_G(), 23 | m_GA(m_G, DPP_GRAPH_ATTRIBUTES), 24 | m_haveSolution(false), 25 | m_returnToInitial(true), 26 | m_cost(0), 27 | m_Tour(), 28 | m_Edges(), 29 | m_algorithm(alg) { 30 | } 31 | 32 | virtual ~PathPlanner() = 0; 33 | 34 | const std::string algorithmName(void) { 35 | return m_algorithm->name(); 36 | } 37 | 38 | Graph *graphPtr(void) { 39 | return &m_G; 40 | } 41 | 42 | GraphAttributes &graphAttributes(void) { 43 | return m_GA; 44 | } 45 | 46 | List &tour(void) { 47 | DPP_ASSERT(m_haveSolution); 48 | return m_Tour; 49 | } 50 | 51 | List &edges(void) { 52 | DPP_ASSERT(m_haveSolution); 53 | return m_Edges; 54 | } 55 | 56 | double cost(void) { 57 | DPP_ASSERT(m_haveSolution); 58 | return m_cost; 59 | } 60 | 61 | int waypointCount(void) { 62 | return m_G.numberOfNodes(); 63 | } 64 | 65 | void returnToInitial(bool value) { 66 | m_returnToInitial = value; 67 | } 68 | 69 | bool returnToInitial(void) { 70 | return m_returnToInitial; 71 | } 72 | 73 | virtual bool solve(void) = 0; 74 | 75 | bool haveSolution(void) { 76 | return m_haveSolution; 77 | } 78 | 79 | protected: 80 | std::unique_ptr m_algorithm; 81 | 82 | Graph m_G; 83 | GraphAttributes m_GA; 84 | List m_Tour; 85 | List m_Edges; 86 | 87 | double m_cost; 88 | bool m_haveSolution; 89 | bool m_returnToInitial; 90 | 91 | }; 92 | 93 | /** 94 | * Base class for a Dubins vehicle path planner (supports constrained turning radius). 95 | */ 96 | class DubinsPathPlanner : public PathPlanner { 97 | public: 98 | 99 | DubinsPathPlanner(double turnRadius, Algorithm *alg) 100 | : m_turnRadius(turnRadius), 101 | m_initialHeading(0.0), 102 | m_Headings(m_G), 103 | PathPlanner(alg) 104 | { } 105 | 106 | virtual ~DubinsPathPlanner() = 0; 107 | 108 | void initialHeading(double x) { 109 | m_initialHeading = x; 110 | } 111 | 112 | double initialHeading(void) { 113 | return m_initialHeading; 114 | } 115 | 116 | double turnRadius(void) { 117 | return m_turnRadius; 118 | } 119 | 120 | void turnRadius(double r) { 121 | m_turnRadius = r; 122 | } 123 | 124 | NodeArray &headings(void) { 125 | DPP_ASSERT(m_Headings.graphOf() == const_cast(&m_G)); 126 | return m_Headings; 127 | } 128 | 129 | void headings(NodeArray &X) { 130 | DPP_ASSERT(X.graphOf() == const_cast(&m_G)); 131 | m_Headings = X; 132 | } 133 | 134 | void copySolution(ogdf::Graph &G, ogdf::GraphAttributes &GA, 135 | ogdf::List &Tour, ogdf::List &Edges, 136 | NodeArray &Headings, double &cost); 137 | 138 | virtual bool solve(void) = 0; 139 | protected: 140 | double m_turnRadius; 141 | double m_initialHeading; 142 | NodeArray m_Headings; 143 | 144 | }; // DubinsPathPlanner 145 | 146 | } // namespace dpp 147 | 148 | #endif // _DPP_PATH_PLANNER_H_ -------------------------------------------------------------------------------- /include/dpp/planner/PathPlannerException.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_PATHPLANNEREXCEPTION_H_ 2 | #define _DPP_PATHPLANNEREXCEPTION_H_ 3 | 4 | #include 5 | 6 | namespace DPP { 7 | 8 | /* 9 | * Exception base class for DPP Exceptions. 10 | */ 11 | class ExceptionBase : public std::exception { }; 12 | 13 | class PathPlannerException : ExceptionBase { }; -------------------------------------------------------------------------------- /include/dpp/planner/WaypointSequencePlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_WAYPOINT_SEQUENCE_PLANNER_H_ 2 | #define _DPP_WAYPOINT_SEQUENCE_PLANNER_H_ 3 | 4 | #include 5 | 6 | namespace dpp { 7 | 8 | #define DPP_SEQUENCE_ID_NOT_SET -1 9 | 10 | typedef struct Waypoint { 11 | double x; 12 | double y; 13 | } Waypoint; 14 | 15 | typedef struct WaypointSequenceTransform { 16 | int oldIndex; 17 | int newIndex; 18 | } WaypointSequenceTransform; 19 | 20 | typedef std::vector WaypointList; 21 | 22 | class WaypointSequencePlanner : private DubinsVehiclePathPlanner { 23 | public: 24 | using DubinsVehiclePathPlanner::DtspPlanningAlgorithm; 25 | using DubinsVehiclePathPlanner::algorithm; 26 | using PathPlanner::algorithmName; 27 | using DubinsPathPlanner::initialHeading; 28 | using DubinsPathPlanner::turnRadius; 29 | using PathPlanner::waypointCount; 30 | using PathPlanner::cost; 31 | using PathPlanner::haveSolution; 32 | 33 | WaypointSequencePlanner(double turnRadius = 1.0, double initialHeading = 0.0, 34 | Algorithm *alg = new AlternatingDtsp) 35 | : DubinsVehiclePathPlanner(turnRadius, alg), 36 | m_sequenceTransformList(m_G) 37 | { 38 | m_initialHeading = initialHeading; 39 | } 40 | 41 | 42 | ~WaypointSequencePlanner() 43 | { } 44 | 45 | bool solve(void) { 46 | return planWaypointSequence(); 47 | } 48 | 49 | bool planWaypointSequence(void); 50 | 51 | int newWaypointSequenceId(int oldIndex); 52 | 53 | std::vector newWaypointSequenceList(void); 54 | 55 | // FIXME move some of these functions up to the PathPlanner abstract class and overload them? 56 | void addWaypoints(const WaypointList list); 57 | 58 | int addWaypoint(const Waypoint waypoint); 59 | 60 | bool containsWaypoint(double x, double y); 61 | 62 | bool containsWaypoint(Waypoint waypoint) { 63 | return containsWaypoint(waypoint.x, waypoint.y); 64 | } 65 | 66 | private: 67 | ogdf::NodeArray m_sequenceTransformList; 68 | std::vector m_originalNodeList, m_newNodeList; 69 | }; 70 | 71 | } // dpp 72 | 73 | #endif // _DPP_WAYPOINT_SEQUENCE_PLANNER_H_ -------------------------------------------------------------------------------- /include/solveCpp.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_SOLVE_CPP_H_ 2 | #define _DPP_SOLVE_CPP_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | int solveCpp(ogdf::DPolygon poly, dpp::VehicleConfiguration C, ogdf::Graph &G, 16 | ogdf::GraphAttributes &GA, double r, double e, ogdf::List &Tour, 17 | ogdf::List &Edges, NodeArray &Headings, double &cost, 18 | bool returnToInitial = true, 19 | dpp::CppPlanningAlgorithm alg=dpp::CppPlanningAlgorithm::BOUSTROPHEDON); 20 | 21 | #endif // _DPP_SOLVE_CPP_H_ -------------------------------------------------------------------------------- /include/solveCppAsDtsp.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_SOLVE_CPP_AS_DTSP_H_ 2 | #define _DPP_SOLVE_CPP_AS_DTSP_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | int solveCppAsDtsp(ogdf::DPolygon poly, dpp::VehicleConfiguration C, ogdf::Graph &G, 16 | ogdf::GraphAttributes &GA, double r, double e, ogdf::List &Tour, 17 | ogdf::List &Edges, NodeArray &Headings, double &cost, 18 | bool returnToInitial = true, 19 | dpp::DtspPlanningAlgorithm alg=dpp::DtspPlanningAlgorithm::ALTERNATING); 20 | 21 | #endif // _DPP_SOLVE_CPP_AS_DTSP_H_ -------------------------------------------------------------------------------- /include/solveDtsp.h: -------------------------------------------------------------------------------- 1 | #ifndef _DPP_SOLVE_DTSP_H_ 2 | #define _DPP_SOLVE_DTSP_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | int solveDtsp(ogdf::Graph &G, ogdf::GraphAttributes &GA, double x, double r, 14 | ogdf::List &Tour, ogdf::List &Edges, NodeArray &Headings, 15 | double &cost, bool returnToInitial = true, 16 | dpp::DtspPlanningAlgorithm alg=dpp::DtspPlanningAlgorithm::ALTERNATING); 17 | 18 | #endif // _DPP_SOLVE_DTSP_H_ -------------------------------------------------------------------------------- /lib/.gitignore: -------------------------------------------------------------------------------- 1 | */ 2 | *.tgz 3 | *.tar.gz 4 | -------------------------------------------------------------------------------- /lib/README.md: -------------------------------------------------------------------------------- 1 | # Project Dependencies 2 | 3 | The following dependencies used by this software are not provided as submodules, and 4 | should be installed with a package manager or manually (LKH): 5 | 6 | * Eigen3 7 | - http://eigen.tuxfamily.org/index.php?title=Main_Page 8 | 9 | * LKH 2.0.7 10 | - http://www.akira.ruc.dk/~keld/research/LKH/ 11 | - http://www.akira.ruc.dk/~keld/research/LKH/LKH-2.0.7.tgz 12 | - *NOTE*: LKH must be compiled manually, and the binary placed in your DPPL/cmake build directory. 13 | 14 | The rest of the dependecies can be obtained by cloning this project recursively 15 | with `git clone --recursive `, or checking out after cloning with 16 | `git submodule update --init --recursive`. They are as follows: 17 | 18 | * cxxopts [2015-09-27] 19 | - https://github.com/jarro2783/cxxopts 20 | 21 | * OGDF [Baobab 2015-06-29.snapshot] 22 | - http://ogdf.net/doku.php 23 | - https://github.com/ogdf/ogdf 24 | 25 | * dubins-curves [2014-03-22] 26 | - https://github.com/dagoodma/Dubins-Curves/tree/cpp, C++ fork of: 27 | - https://github.com/AndrewWalker/Dubins-Curves 28 | - *NOTE*: This library was modified and compiled into a static C++ library. 29 | 30 | * stacktrace.h [2008] 31 | - http://panthema.net/2008/0901-stacktrace-demangled/ 32 | - *NOTE*: This is already included with the DPPL code, and it is only used in the 33 | project binaries for debugging purposes. 34 | - *TODO*: This does not demangle symbols on Mac OS X. For that, see here: 35 | https://github.com/MesserLab/SLiM/search?utf8=%E2%9C%93&q=eidos_print_stacktrace 36 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists 2 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) 3 | set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}) 4 | 5 | ################################ 6 | # DPP Library 7 | ################################ 8 | add_library(DPP STATIC 9 | dpp/basic/basic.cpp 10 | dpp/basic/Logger.cpp 11 | dpp/basic/Path.cpp 12 | dpp/basic/Field.cpp 13 | dpp/basic/FieldTrack.cpp 14 | dpp/basic/FileIO.cpp 15 | dpp/planalg/Algorithm.cpp 16 | dpp/planalg/RandomizedDtsp.cpp 17 | dpp/planalg/NearestNeighborDtsp.cpp 18 | dpp/planalg/AlternatingDtsp.cpp 19 | dpp/planalg/BoustrophedonCpp.cpp 20 | dpp/planner/PathPlanner.cpp 21 | dpp/planner/DubinsVehiclePathPlanner.cpp 22 | dpp/planner/DubinsSensorPathPlanner.cpp 23 | dpp/planner/WaypointSequencePlanner.cpp 24 | dpp/planner/CoverageWaypointPlanner.cpp 25 | ) 26 | 27 | TARGET_LINK_LIBRARIES( DPP ${OGDF_LIBRARIES} DUBINSCURVES ) 28 | 29 | ################################ 30 | # Binaries 31 | ################################ 32 | 33 | # Solve DTSP binary 34 | ADD_EXECUTABLE( solveDtsp solveDtsp.cpp ) 35 | TARGET_LINK_LIBRARIES( solveDtsp ${OGDF_LIBRARIES} DPP DUBINSCURVES ) 36 | 37 | # Solve CPP as DTSP binary 38 | ADD_EXECUTABLE( solveCppAsDtsp solveCppAsDtsp.cpp ) 39 | TARGET_LINK_LIBRARIES( solveCppAsDtsp ${OGDF_LIBRARIES} DPP DUBINSCURVES ) 40 | 41 | # Solve CPP binary 42 | ADD_EXECUTABLE( solveCpp solveCpp.cpp ) 43 | TARGET_LINK_LIBRARIES( solveCpp ${OGDF_LIBRARIES} DPP DUBINSCURVES ) 44 | 45 | -------------------------------------------------------------------------------- /src/computeDubinsDistancesATSP.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License 3 | Copyright (c) 2015 UCSC Autonomous Systems Lab 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 18 | THE SOFTWARE. 19 | */ 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | //#include 27 | 28 | #include "Log.h" 29 | #include "Dubins.h" 30 | #include "Configuration.h" 31 | #include "Util.h" 32 | #include "NodeMatrix.h" 33 | #include "TSPLib.h" 34 | 35 | 36 | //#define DEBUG 37 | 38 | // Enable stack traces in debug mode 39 | #ifdef DEBUG 40 | #include "stacktrace.h" 41 | #endif 42 | 43 | #define DEFAULT_TURN_RADIUS 10.0 // [m] 44 | 45 | using namespace std; 46 | 47 | // Have to import specifially, since Configuration clashes 48 | using ogdf::node; 49 | using ogdf::Graph; 50 | using ogdf::GraphAttributes; 51 | using ogdf::GraphCopy; 52 | using ogdf::DPoint; 53 | using ogdf::List; 54 | using ogdf::ListIterator; 55 | using ogdf::NodeArray; 56 | 57 | /** 58 | * Writes Dubins edge distances to a TSPLIB-style problem file (.tsp). 59 | */ 60 | int main(int argc, char *argv[]) { 61 | // Setup stack traces for debugging 62 | char const *program_name = argv[0]; 63 | #ifdef DEBUG 64 | set_signal_handler(program_name); 65 | #endif 66 | 67 | // Set option parsing 68 | //cxxopts::Options opts(program_name, " - computes a weighted adjacency matrix with Dubins' edge costs"); 69 | 70 | // Initialize logging 71 | FILELog::ReportingLevel() = logDEBUG3; 72 | FILE* log_fd = fopen( "logfile.txt", "w" ); 73 | Output2FILE::Stream() = log_fd; 74 | FILE_LOG(logDEBUG) << "Started."; 75 | 76 | // Read arguments 77 | if (argc != 3) { 78 | cerr << "Expected 2 arguments." << endl; 79 | return 1; 80 | } 81 | string inputFilename(argv[1]), outputFilename(argv[2]); 82 | 83 | // Read input gml file 84 | Graph G; 85 | GraphAttributes GA(G, 86 | GraphAttributes::nodeGraphics | 87 | GraphAttributes::edgeGraphics | 88 | GraphAttributes::nodeLabel | 89 | GraphAttributes::edgeStyle | 90 | GraphAttributes::nodeStyle | 91 | GraphAttributes::nodeTemplate | 92 | GraphAttributes::nodeId); 93 | 94 | if (!ogdf::GraphIO::readGML(GA, G, inputFilename)) { 95 | cerr << "Could not open " << inputFilename << endl; 96 | return 1; 97 | } 98 | 99 | int m = G.numberOfEdges(); 100 | int n = G.numberOfNodes(); 101 | FILE_LOG(logDEBUG) << "Opened " << inputFilename << ". Found " << m << " edges, and " 102 | << n << " nodes." << endl; 103 | 104 | // Build Dubins' weighted adjacency matrix 105 | //cout << "Here at line " << __LINE__ << " in file " << __FILE__ << "." << endl; 106 | NodeMatrix A(G); 107 | NodeArray heading(G,0.0); // (TODO: randomize) 108 | buildDubinsAdjacencyMatrix(G, GA, A, heading, DEFAULT_TURN_RADIUS); 109 | 110 | node v = G.firstNode(); 111 | //cout << "At node v: " << A[v][v] << "." << endl; 112 | 113 | // Find nearest neighbor solution 114 | FILE_LOG(logDEBUG) << "Starting Dubins' adjacency matrix computation." << endl; 115 | FILE_LOG(logDEBUG) << "Finished." << endl; 116 | cout << "Computed weighted adjacency matrix." << endl; 117 | 118 | // Write solution to .tsp file 119 | size_t pos = outputFilename.find(".tsp"); 120 | string problemName(outputFilename,0,pos); 121 | string problemComment("Dubins' path for "); 122 | problemComment += to_string(n) + " point scenario."; 123 | writeATSPFile(outputFilename, problemName, problemComment, G, A); 124 | 125 | return 0; 126 | } 127 | 128 | -------------------------------------------------------------------------------- /src/dpp/basic/FieldTrack.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Implementation of dpp::FieldTrack class for representing a track on a field for 3 | * vehicle traversal. The track is a line that contains 2 nodes on a graph as endpoints. 4 | * 5 | * Copyright (C) 2014-2015 DubinsPathPlanner. 6 | * Created by David Goodman 7 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 8 | * For details see the LICENSE file distributed with DubinsPathPlanner. 9 | */ 10 | 11 | #include 12 | 13 | namespace dpp { 14 | 15 | using ogdf::node; 16 | using ogdf::edge; 17 | 18 | ogdf::node addNodeToGraph(ogdf::Graph &G, ogdf::GraphAttributes &GA, DPoint p) { 19 | //int newNodeId = G.maxNodeIndex() + 1; // origin id=1 should already be in the graph 20 | int newNodeId = (G.maxNodeIndex() < 0)? 1 21 | : G.maxNodeIndex() + 2; // indexing starts from 1 or else MATLAB and maybe QGC will crash 22 | 23 | node u = G.newNode(); 24 | GA.x(u) = p.m_x; 25 | GA.y(u) = p.m_y; 26 | GA.idNode(u) = newNodeId; 27 | 28 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Added node " << newNodeId 29 | << ": " << p << " to the graph." << std::endl; 30 | 31 | return u; 32 | } 33 | 34 | 35 | int addNodesFromFieldTrack(const FieldTrack &t, ogdf::Graph &G, 36 | ogdf::GraphAttributes &GA, double distance, ogdf::List &Tour, 37 | ogdf::NodeArray &Headings) { 38 | int n = 0; 39 | 40 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Adding nodes to cover field track: " 41 | << t << std::endl; 42 | 43 | double angle = (!t.reverse())? t.angle() 44 | : wrapAngle(t.angle() + M_PI); 45 | 46 | // Add the start node 47 | //DPoint p = (!t.reverse())? t.start() : t.end(); 48 | DPoint p = t.start(); 49 | Tour.pushBack(addNodeToGraph(G, GA, p)); 50 | Headings(Tour.back()) = angleToHeading(angle); 51 | n++; 52 | 53 | // Add nodes along the track, distance apart 54 | if (t.length() > distance) { 55 | p += polarToCartesian(angle, distance); 56 | while(t.contains(p) && p != t.end()) { 57 | Tour.pushBack(addNodeToGraph(G, GA, p)); 58 | Headings(Tour.back()) = angleToHeading(angle); 59 | n++; 60 | 61 | p += polarToCartesian(angle, distance); 62 | } 63 | 64 | } 65 | 66 | // Add the end node 67 | if (t.length() >= distance) { 68 | //p = (!t.reverse())? t.start() : t.end(); 69 | Tour.pushBack(addNodeToGraph(G, GA, t.end())); 70 | Headings(Tour.back()) = angleToHeading(angle); 71 | n++; 72 | } 73 | 74 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Added " << n 75 | << " nodes to cover field track." << std::endl; 76 | 77 | return n; 78 | } 79 | 80 | 81 | /** 82 | * Decomposes the list into nodes placed distance apart, and adds them to the 83 | * graph. Builds a tour with headings. 84 | * @param[in] G graph to add nodes to 85 | * @param[in] GA graph attributes 86 | * @param[in] distance between nodes along each track 87 | * @param[out] Tour ordered list to save the nodes 88 | * @param[out] Headings to save node heading along track 89 | * @return the number of nodes added to the graph 90 | * @remark FIXME does not check if graph contains identical nodes 91 | */ 92 | int FieldTrackList::addNodesToGraph(ogdf::Graph &G, ogdf::GraphAttributes &GA, 93 | double distance, ogdf::List &Tour, 94 | ogdf::NodeArray &Headings) const { 95 | int n = 0; 96 | // Clear the tour and add the origin node 97 | Tour.clear(); 98 | Tour.pushBack(G.firstNode()); 99 | //Headings.init(G); 100 | 101 | // Iterate over the field tracks and create nodes 102 | FieldTrackListConstIterator iter; 103 | for (iter = begin(); iter != end(); iter++) { 104 | const FieldTrack t = *iter; 105 | n += addNodesFromFieldTrack(t, G, GA, distance, Tour, Headings); 106 | } 107 | return n; 108 | } 109 | 110 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/basic/Logger.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Single Logger class implementation that uses ogdf::Logger and supports a log file. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | #include 10 | 11 | #include 12 | 13 | namespace dpp { 14 | 15 | // Global static pointer used to ensure a single instance of the class. 16 | Logger* Logger::m_pInstance = nullptr; 17 | std::ostream Logger::nullStream(0); 18 | 19 | /* 20 | * Initialize logging for default stream. 21 | */ 22 | Logger* Logger::Instance(void) 23 | { 24 | // Only allow one instance of class to be generated. 25 | if (!m_pInstance) { 26 | m_pInstance = new Logger; 27 | } 28 | 29 | return m_pInstance; 30 | } 31 | 32 | /* 33 | * Deconstructor for log file. Closes file, and destructs ogdf::Logger. 34 | */ 35 | Logger::~Logger() { 36 | if (m_useFile && m_logFile.is_open()) { 37 | m_logFile.close(); 38 | } 39 | m_pInstance = nullptr; 40 | } 41 | 42 | /* 43 | * Initialize logging for writing to an output stream. 44 | */ 45 | void Logger::initializeLogger(std::ostream &ostream) { 46 | Logger *log = Logger::Instance(); 47 | log->m_logger.setWorldStream(ostream); 48 | log->m_logger.lout() << "Log initialized." << endl; 49 | } 50 | 51 | /* 52 | * Initialize logging for writing to an output file. 53 | */ 54 | void Logger::initializeLogger(std::string logFilename) { 55 | Logger *log = Logger::Instance(); 56 | if (log->m_logFile.is_open()) { 57 | log->m_logFile.close(); 58 | } 59 | log->m_logFile.open(logFilename, std::fstream::out | std::fstream::trunc); 60 | log->m_useFile = true; 61 | initializeLogger(log->m_logFile); 62 | } 63 | 64 | std::ostream & Logger::logOut(Logger::Level l, Logger::Verbosity v) { 65 | Logger *log = Logger::Instance(); 66 | 67 | if (log->m_verbosity >= v) { 68 | if (log->m_level <= l) { 69 | return log->m_logger.lout(); 70 | } 71 | } 72 | return nullStream; 73 | } 74 | 75 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/basic/basic.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * basic feature implementation 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | 10 | #include 11 | 12 | namespace dpp { 13 | 14 | 15 | } // namespace DPP -------------------------------------------------------------------------------- /src/dpp/planalg/Algorithm.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Implementation of DPP::Algorithm abstract class. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | // For unix we use popen, TODO remove this 15 | #if defined (__unix__) || (defined (__APPLE__) && defined (__MACH__)) 16 | 17 | //#include 18 | 19 | #endif 20 | 21 | 22 | namespace dpp { 23 | 24 | const char* Algorithm::TypeText[] = { "Dtsp", "CPP" }; 25 | 26 | Algorithm::~Algorithm() { 27 | 28 | } 29 | 30 | 31 | /** 32 | * Makes a system call to run the LKH solver. 33 | * @throws a runtime exception if the solver fails. 34 | * @return SUCCESS or FAILURE 35 | * @TODO create a new thread, capture output 36 | */ 37 | int Algorithm::runLKHSolver(std::string parFilename) { 38 | int result = FAILURE; 39 | std::string shell_command = LKH_EXECUTABLE " " + parFilename; 40 | 41 | #if defined (__unix__) || (defined (__APPLE__) && defined (__MACH__)) 42 | #include 43 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Running LKH Solver with popen(): " 44 | << const_cast(shell_command.c_str()) << std::endl; 45 | FILE *pFile = popen(const_cast(shell_command.c_str()), "r"); 46 | // Read through stdout until it ends 47 | // TODO add error checking/parsing here 48 | char buffer [100]; 49 | while ( ! feof (pFile) ) 50 | { 51 | if ( fgets (buffer , 100 , pFile) == NULL ) break; 52 | //fputs (buffer , stdout); 53 | } 54 | 55 | result = pclose(pFile); 56 | // Additional error reporting in Unix 57 | if (result != SUCCESS) { 58 | int errsv = errno; 59 | Logger::logError() << "Failed with error code: " << errsv << std::endl; 60 | } 61 | #else 62 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Running LKH Solver with system(): " 63 | const_cast(cmd.c_str()) << std::endl; 64 | // If we don't have open, stdout is printed to terminal 65 | result = system(const_cast(cmd.c_str())); 66 | #endif 67 | 68 | if (result != SUCCESS) { 69 | throw std::runtime_error(string("LKH solver failed with code: ") + to_string(result)); 70 | } 71 | 72 | return result; 73 | } 74 | 75 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/planalg/BoustrophedonCpp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Implementation of dpp::BoustrophdeonCpp class for solving CPP problems 3 | * by generating field tracks that cover a convex field, and decomposing 4 | * the tracks into waypoints by joining the tracks end-to-end. 5 | * 6 | * Copyright (C) 2014-2015 DubinsPathPlanner. 7 | * Created by David Goodman 8 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 9 | * For details see the LICENSE file distributed with DubinsPathPlanner. 10 | */ 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace dpp { 18 | 19 | /** 20 | * Order field tracks in the track list according to the Boustrophedon CPP 21 | * algorithm. Track direction (reverse or not) will be set. 22 | * @remark This sets the tracks to reversed in an alternating order. 23 | * @param[in] tracks field track list to reorder 24 | * @param[in] startAtLast whether to use the last track as the first 25 | * @param[in] reverseStartTrack whether to start in the reverse direction 26 | * on the first track (or last depending). 27 | */ 28 | void setFieldTrackOrder(FieldTrackList &tracks, bool startAtLast, 29 | bool reverseStartTrack) { 30 | if (startAtLast) { 31 | tracks.reverse(); 32 | } 33 | 34 | bool directionReversed = reverseStartTrack; 35 | FieldTrackListIterator iter; 36 | for (iter = tracks.begin(); iter != tracks.end(); iter++) { 37 | (*iter).reverse(directionReversed); 38 | directionReversed ^= 1; // flip direction 39 | } 40 | } 41 | 42 | /** 43 | * Solves the CPP problem using the Boustrophedon algorithm. 44 | * @param[in] field a coverage field to generate waypoints over 45 | * @param[in] C initial configuration of the vehicle 46 | * @param[in] turnRadius of the vehicle 47 | * @param[in] returnToInitial whether to include return edge and cost 48 | * @param[out] G a graph to populate with nodes for coverage 49 | * @param[out] GA attributes of the graph 50 | * @param[out] Tour an ordered list of nodes representing the path 51 | * @param[out] Edges an ordered list of edges representing the path 52 | * @param[out] Headings headings used along the path at each node 53 | * @param[out] cost overall cost (path length) for covering the field 54 | * @return SUCCESS or FAILURE of the algorithm 55 | */ 56 | int BoustrophedonCpp::run(const Field &field, VehicleConfiguration C, double turnRadius, 57 | bool returnToInitial, Graph &G, GraphAttributes &GA, List &Tour, 58 | List &Edges, NodeArray &Headings, double &cost) { 59 | 60 | // Check arguments 61 | if (Headings.graphOf() != &G) { 62 | throw std::domain_error("Headings should be for G."); 63 | } 64 | 65 | // Generate field tracks 66 | FieldTrackList tracks; 67 | if (field.generateFieldTracks(tracks) < 1) { 68 | throw std::runtime_error("Failed to generate field tracks."); 69 | } 70 | 71 | // Find the closest endpoint among first or last track from initial config 72 | bool startAtLast = false; 73 | bool reverseStartTrack = false; 74 | 75 | FieldTrack firstTrack = tracks.front(); 76 | FieldTrack lastTrack = tracks.back(); 77 | VehicleConfiguration 78 | CfirstStart(firstTrack.start().m_x, firstTrack.start().m_y, firstTrack.angle()), 79 | CfirstEnd(firstTrack.end().m_x, firstTrack.end().m_y, firstTrack.angle(true)), 80 | ClastStart(lastTrack.start().m_x, lastTrack.start().m_y, lastTrack.angle()), 81 | ClastEnd(lastTrack.end().m_x, lastTrack.end().m_y, lastTrack.angle(true)); 82 | 83 | double d_fs = dubinsPathLength(C, CfirstStart, turnRadius), 84 | d_fe = dubinsPathLength(C, CfirstEnd, turnRadius), 85 | d_ls = dubinsPathLength(C, ClastStart, turnRadius), 86 | d_le = dubinsPathLength(C, ClastEnd, turnRadius); 87 | double d_min = d_fs; 88 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Comparing distances: " << std::endl 89 | << "First track: " << d_fs << " and " << d_fe << std::endl 90 | << "Last track: " << d_ls << " and " << d_le << std::endl; 91 | 92 | if (d_fe < d_min) { 93 | d_min = d_fe; 94 | reverseStartTrack = true; 95 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Chose first track end: " << firstTrack.end() << std::endl; 96 | } 97 | else if (d_ls < d_min) { 98 | d_min = d_ls; 99 | reverseStartTrack = false; 100 | startAtLast = true; 101 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Chose last track start: " << lastTrack.start() << std::endl; 102 | } 103 | else if (d_le < d_min) { 104 | d_min = d_le; 105 | reverseStartTrack = true; 106 | startAtLast = true; 107 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Chose last track end: " << lastTrack.end() << std::endl; 108 | } 109 | else { 110 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Chose first track start: " << firstTrack.start() << std::endl; 111 | } 112 | 113 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Decomposing " << tracks.size() 114 | << " field tracks into nodes with coverage width:" << field.coverageWidth() 115 | << std::endl; 116 | 117 | setFieldTrackOrder(tracks, startAtLast, reverseStartTrack); 118 | int n = tracks.addNodesToGraph(G, GA, field.coverageWidth(), Tour, Headings); 119 | 120 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Added " << n 121 | << " nodes to cover entire field." << std::endl; 122 | 123 | // Create edges 124 | cost = createDubinsTourEdges(G, GA, Tour, Headings, turnRadius, Edges, returnToInitial); 125 | 126 | return SUCCESS; 127 | } 128 | 129 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/planalg/NearestNeighborDtsp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Implementation of DPP::NearestNeighbor class for solving DTSP problems 3 | * with the nearest neighbor algorithm. 4 | * 5 | * Copyright (C) 2014-2015 DubinsPathPlanner. 6 | * Created by David Goodman 7 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 8 | * For details see the LICENSE file distributed with DubinsPathPlanner. 9 | */ 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using ogdf::GraphCopy; 19 | 20 | namespace dpp { 21 | /** 22 | * Find the copied node vC nearest to C using the shortest Dubins path distance metric. 23 | */ 24 | double findNearestNode(GraphCopy &GC, GraphAttributes &GA, VehicleConfiguration &C, 25 | node &vC, double r) { 26 | double minDist = -1.0f; 27 | node iC; 28 | forall_nodes(iC,GC) { 29 | node i = GC.original(iC); 30 | 31 | VehicleConfiguration Ci(GA.x(i), GA.y(i)); 32 | //double dist = C.euclideanDistance(Ci); 33 | Ci.heading(headingBetween(C.asVector(), Ci.asVector()) ); 34 | double dist = dubinsPathLength(C, Ci, r); 35 | if (minDist < 0.0f || dist < minDist) { 36 | vC = iC; 37 | minDist = dist; 38 | } 39 | } 40 | 41 | return minDist; 42 | } 43 | 44 | /** 45 | * Solves the Dtsp with the nearest neighbor algorithm. The tour, headings, and total 46 | * cost are saved into their respective parameters. 47 | * @param G a graph of the problem 48 | * @param GA attributes of graph 49 | * @param x a starting heading in radians [0,2*pi) 50 | * @param r a turning radius in radians 51 | * @param Tour a list of nodes to hold the result 52 | * @param Edges a list of edges to hold the result 53 | * @param Headings a node array of headings to hold the result 54 | * @param cost holds the total cost result 55 | * @return An exit code (0==SUCCESS) 56 | */ 57 | int NearestNeighborDtsp::run(Graph &G, GraphAttributes &GA, double x, double r, 58 | List &Tour, List &Edges, NodeArray &Headings, double &cost, 59 | bool returnToInitial) { 60 | // Check arguments 61 | if (x < 0.0 || x >= M_PI*2.0) { 62 | throw std::out_of_range("Expected x to be between 0 and 2*PI"); 63 | } 64 | 65 | if (Headings.graphOf() != &G) { 66 | throw std::domain_error("Headings should be for G"); 67 | } 68 | 69 | if (G.numberOfNodes() < 2) { 70 | throw std::out_of_range("Expected G to have at least 2 nodes"); 71 | } 72 | if (G.numberOfEdges() > 0) { 73 | throw std::out_of_range("Expected G to have 0 edges"); 74 | } 75 | 76 | int m = G.numberOfEdges(); 77 | int n = G.numberOfNodes(); 78 | 79 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Found " << n << " nodes, and " << m << " edges." << std::endl; 80 | 81 | // Make a graph copy and remove nodes until none are left 82 | double total_cost = 0.0f; 83 | GraphCopy GC(G); // unvisited nodes 84 | node nodeStart = G.firstNode(); 85 | 86 | // Start at the first node 87 | node u = nodeStart; 88 | VehicleConfiguration Cs(GA.x(nodeStart), GA.y(nodeStart), x); 89 | VehicleConfiguration C(Cs); 90 | 91 | // Remove the first node (add return edge afterwards) 92 | GC.delNode(GC.copy(nodeStart)); 93 | Tour.pushBack(nodeStart); 94 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << printGraph(G,GA) << std::endl; 95 | 96 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) 97 | << "Running nearest neighbor solver for Greedy TSP." << std::endl; 98 | Timer *t1 = new Timer(); 99 | while (!GC.empty()) { 100 | // Find the closest node, and remove it from the graph 101 | node vC; 102 | double cost = findNearestNode(GC, GA, C, vC, r); 103 | total_cost += cost; 104 | node v = GC.original(vC); 105 | GC.delNode(vC); 106 | 107 | // Create an edge, and add it to the tour 108 | edge e = G.newEdge(u, v); 109 | GA.doubleWeight(e) = cost; 110 | Edges.pushBack(e); 111 | Tour.pushBack(v); 112 | 113 | // Use the heading of line segment between two nodes 114 | Vector2d uv = C.asVector(); 115 | Vector2d vv(GA.x(v), GA.y(v)); 116 | double x_v = headingBetween(uv,vv); 117 | Headings(v) = x_v; 118 | u = v; 119 | 120 | // Update our current position 121 | C.position(GA.x(v), GA.y(v)); 122 | C.heading(x_v); 123 | } 124 | 125 | // Return to start configuration 126 | if (returnToInitial) { 127 | double cost_return = dubinsPathLength(C, Cs, r); 128 | cost = total_cost + cost_return; 129 | edge e = G.newEdge(u, nodeStart); 130 | GA.doubleWeight(e) = cost_return; 131 | Edges.pushBack(e); 132 | Tour.pushBack(nodeStart); 133 | } 134 | 135 | // Create edges 136 | cost = dubinsTourCost(G, GA, Tour, Headings, r, returnToInitial); 137 | 138 | // Debug print info 139 | float elapsedTime = t1->diffMs(); 140 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Solved " << G.numberOfNodes() 141 | << " point tour with cost " << cost << " (" << elapsedTime << " ms)." << std::endl; 142 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << dpp::printHeadings(G, GA, Headings); 143 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << dpp::printTour(G, GA, Tour); 144 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << dpp::printEdges(G, GA, Edges); 145 | 146 | return SUCCESS; 147 | } // NearestNeighborDtsp::run() 148 | 149 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/planner/CoverageWaypointPlanner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Simplified implementation of DubinsSensorPathPlanner. Does not require OGDF 3 | * objects. Only standard containers and the ones defined here. 4 | * 5 | * Copyright (C) 2014-2015 DubinsPathPlanner. 6 | * Created by David Goodman 7 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 8 | * For details see the LICENSE file distributed with DubinsPathPlanner. 9 | */ 10 | #include 11 | #include 12 | #include 13 | 14 | namespace dpp { 15 | 16 | int CoverageWaypointPlanner::addPolygonVertices(VertexList list) { 17 | DPP_ASSERT(list.size() > 0); 18 | //m_G.clear(); 19 | m_waypointList.clear(); 20 | m_haveSolution = false; 21 | int n = 0; 22 | 23 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Adding list of vertices: " << std::endl; 24 | for (const auto& v : list) { 25 | addPolygonVertex(v); 26 | n++; 27 | } 28 | 29 | return vertexCount(); 30 | } 31 | 32 | void CoverageWaypointPlanner::addPolygonVertex(Vertex v) { 33 | ogdf::DPoint p(v.x, v.y); 34 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Adding point (" << p.m_x << ", " 35 | << p.m_y << ") to polygon." << std::endl; 36 | //if (!m_polygon.containsPoint(p)) { 37 | m_polygon.pushBack(p); 38 | //} 39 | } 40 | 41 | /** 42 | * Find waypoints that efficiently cover the designated area. 43 | * @return true if the planner succeeded 44 | * @remark Call waypointList() afterwards to retreive the solution. 45 | */ 46 | bool CoverageWaypointPlanner::planCoverageWaypoints(void) { 47 | DPP_ASSERT(vertexCount() >= 3); 48 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Planning coverage waypoint path for polygon with " 49 | << vertexCount() << " vertices." << std::endl; 50 | 51 | //if(!solveAsDtsp()) { 52 | // return false; 53 | //} 54 | if(!DubinsSensorPathPlanner::solve()) { 55 | Logger::logError() << "Failed to find solution." << std::endl; 56 | return false; 57 | } 58 | 59 | // Build waypoint list from tour 60 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Building waypoint list:" << std::endl; 61 | ogdf::ListIterator tourIter; 62 | for ( tourIter = m_Tour.begin(); tourIter != m_Tour.end(); tourIter++ ) { 63 | ogdf::node u = *tourIter; 64 | 65 | // Skip adding the origin to the end of the list 66 | if (u == m_G.firstNode() && tourIter.succ() == m_Tour.end()) 67 | continue; 68 | 69 | Waypoint w = {m_GA.x(u), m_GA.y(u)}; 70 | m_waypointList.push_back(w); 71 | 72 | Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Node " << u << " id=" << m_GA.idNode(u) << ", x=" 73 | << w.x << ", y=" << w.y << std::endl; 74 | } 75 | 76 | return true; 77 | } 78 | 79 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/planner/DubinsSensorPathPlanner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Path planner for a Dubins vehicle with a sensor. Considers the 2D turning radius 3 | * of the vehicle and the coverage width of the sensor. 4 | * 5 | * Copyright (C) 2014-2015 DubinsPathPlanner. 6 | * Created by David Goodman 7 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 8 | * For details see the LICENSE file distributed with DubinsPathPlanner. 9 | */ 10 | #include 11 | #include 12 | #include 13 | 14 | namespace dpp { 15 | 16 | void DubinsSensorPathPlanner::polygon(std::string gmlFilename) { 17 | DPP_ASSERT(readPolygonFromGmlFile(gmlFilename, m_polygon) == SUCCESS); 18 | 19 | polygon(m_polygon); // for logger message 20 | } 21 | 22 | void DubinsSensorPathPlanner::polygon(DPolygon polygon) { 23 | m_polygon = polygon; 24 | 25 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Set polygon to " << &polygon << 26 | " with n=" << polygon.size() << " points." << std::endl; 27 | } 28 | 29 | void DubinsSensorPathPlanner::polygon(List points) { 30 | Logger::logWarn() << "polygon(List) not implemented!" << std::endl; 31 | } 32 | 33 | 34 | /** 35 | * Sets the CPP algorithm for solving. 36 | */ 37 | void DubinsSensorPathPlanner::algorithm(CppPlanningAlgorithm algId) { 38 | switch (algId) { 39 | case BOUSTROPHEDON: 40 | m_algorithm.reset(new BoustrophedonCpp()); 41 | Logger::logDebug() << "Set CPP algorithm to BOUSTROPHEDON." << std::endl; 42 | break; 43 | default: 44 | DPP_ASSERT(0 && "Unknown planning algorithm"); 45 | Logger::logError() << "Unknown planning algorithm: " << algId << std::endl; 46 | return; 47 | } // switch (algId) 48 | } 49 | 50 | /** 51 | * Find the DTSP solution to the CPP problem. 52 | */ 53 | bool DubinsSensorPathPlanner::solveAsDtsp(DtspPlanningAlgorithm algId) { 54 | DPP_ASSERT(m_polygon.size() >= 3); 55 | m_haveSolution = false; 56 | 57 | try { 58 | // Create a DTSP path planner and copy settings 59 | DubinsVehiclePathPlanner p(m_turnRadius); 60 | p.algorithm(algId); 61 | p.initialHeading(m_initialHeading); 62 | p.turnRadius(m_turnRadius); 63 | p.returnToInitial(m_returnToInitial); 64 | 65 | Logger::logInfo(DPP_LOGGER_VERBOSE_1) << "Solving coverage problem with " 66 | << p.algorithmName() << " DTSP algorithm." << std::endl; 67 | 68 | // Clear and add origin to graph with heading 69 | m_G.clear(); 70 | ogdf::node nodeStart = m_G.newNode(); 71 | m_GA.x(nodeStart) = m_initialConfig.x(); 72 | m_GA.y(nodeStart) = m_initialConfig.y(); 73 | m_GA.idNode(nodeStart) = 1; 74 | m_Headings(m_G.firstNode()) = m_initialConfig.heading(); 75 | 76 | // Construct field and grid polygon by sensor width, 77 | // building a graph with centroids as nodes 78 | Field field(m_polygon, m_sensorWidth); 79 | int n = field.addNodesFromGrid(m_G, m_GA); 80 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Added " << n << " nodes " 81 | << "to the Graph." << std::endl; 82 | 83 | // Solve as DTSP problem 84 | p.addWaypoints(m_G, m_GA); 85 | if (p.solve() == true) { 86 | m_haveSolution = true; 87 | p.copySolution(m_G, m_GA, m_Tour, m_Edges, m_Headings, m_cost); 88 | } 89 | } catch(std::exception &e) { 90 | Logger::logError() << "An exception occured: " << e.what() << std::endl; 91 | m_haveSolution = false; 92 | } 93 | return m_haveSolution; 94 | } 95 | 96 | bool DubinsSensorPathPlanner::solve(void) { 97 | DPP_ASSERT(m_polygon.size() >= 3); 98 | m_haveSolution = false; 99 | 100 | // Clear and add origin to graph with heading 101 | m_G.clear(); 102 | ogdf::node nodeStart = m_G.newNode(); 103 | m_GA.x(nodeStart) = m_initialConfig.x(); 104 | m_GA.y(nodeStart) = m_initialConfig.y(); 105 | m_GA.idNode(nodeStart) = 1; 106 | m_Headings(m_G.firstNode()) = m_initialConfig.heading(); 107 | 108 | Logger::logInfo(DPP_LOGGER_VERBOSE_1) << "Solving coverage problem with " 109 | << m_algorithm->name() << " " << m_algorithm->typeText() << " algorithm." << std::endl; 110 | try { 111 | // Generate the field 112 | Field field(m_polygon, m_sensorWidth); 113 | if (!field.isConvex()) { 114 | throw std::domain_error("Non-convex polygons not yet supported."); 115 | } 116 | 117 | // Call the algorithm 118 | AlgorithmCpp *alg = dynamic_cast(m_algorithm.get()); 119 | if (alg->run(field, m_initialConfig, m_turnRadius, m_returnToInitial, 120 | m_G, m_GA, m_Tour, m_Edges, m_Headings, m_cost) == SUCCESS) { 121 | m_haveSolution = true; 122 | } 123 | } catch(std::exception &e) { 124 | Logger::logError() << "An exception occured: " << e.what() << std::endl; 125 | m_haveSolution = false; 126 | } 127 | return m_haveSolution; 128 | } 129 | 130 | 131 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/planner/DubinsVehiclePathPlanner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Path planner for a Dubins vehicle. Considers the 2D turning radius of the vehicle. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | #include 10 | 11 | #include 12 | 13 | namespace dpp { 14 | 15 | /* 16 | * Load graph from GML files. 17 | */ 18 | void DubinsVehiclePathPlanner::addWaypoints(std::string gmlFilename) { 19 | DPP_ASSERT(readGraphFromGmlFile(gmlFilename, m_G, m_GA) == SUCCESS); 20 | m_Headings.init(m_G); 21 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Read " << gmlFilename << " with " << waypointCount() 22 | << " nodes." << std::endl; 23 | } 24 | 25 | /* 26 | * Copy the given Graph and attributes to add waypoints. 27 | * @note Existing waypoints are deleted. 28 | */ 29 | void DubinsVehiclePathPlanner::addWaypoints(ogdf::Graph &G, ogdf::GraphAttributes &GA) { 30 | DPP_ASSERT(G.numberOfEdges() == 0); // TODO just clear edges 31 | m_G.clear(); 32 | // Do we need to worry about resetting attributes too? 33 | //m_GA.init(m_G, m_GA.attributes()); 34 | 35 | // Copy graph and attributes 36 | int n = copyGraph(G, GA, m_G, m_GA); 37 | m_Headings.init(m_G); 38 | 39 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Copied " << n << " nodes from graph " << &G << " with " << waypointCount() 40 | << " nodes into graph " << &m_G << std::endl; 41 | } 42 | 43 | /* 44 | * Run the algorithm to solve the planning problem. 45 | */ 46 | bool DubinsVehiclePathPlanner::solve(void) { 47 | DPP_ASSERT(waypointCount() > 1); 48 | m_haveSolution = false; 49 | 50 | // Set the initial heading 51 | m_Headings(m_G.firstNode()) = m_initialHeading; 52 | 53 | Logger::logInfo(DPP_LOGGER_VERBOSE_1) << "Solving " << waypointCount() << " node problem with " 54 | << m_algorithm->name() << " " << m_algorithm->typeText() << " algorithm." << std::endl; 55 | try { 56 | AlgorithmDtsp *alg = dynamic_cast(m_algorithm.get()); 57 | 58 | if (alg->run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, 59 | m_Edges, m_Headings, m_cost, m_returnToInitial) == SUCCESS) { 60 | m_haveSolution = true; 61 | } 62 | } catch(std::exception &e) { 63 | Logger::logError() << "An exception occured: " << e.what() << std::endl; 64 | m_haveSolution = false; 65 | } 66 | 67 | return m_haveSolution; 68 | } 69 | 70 | /* 71 | * Sets the DTSP algorithm for solving. 72 | */ 73 | void DubinsVehiclePathPlanner::algorithm(DtspPlanningAlgorithm algId) { 74 | switch (algId) { 75 | case NEAREST_NEIGHBOR: 76 | m_algorithm.reset(new NearestNeighborDtsp()); 77 | Logger::logDebug() << "Set DTSP algorithm to NEAREST_NEIGHBOR." << std::endl; 78 | break; 79 | case ALTERNATING: 80 | m_algorithm.reset(new AlternatingDtsp()); 81 | Logger::logDebug() << "Set DTSP algorithm to ALTERNATING." << std::endl; 82 | break; 83 | case RANDOMIZED: 84 | m_algorithm.reset(new RandomizedDtsp()); 85 | Logger::logDebug() << "Set DTSP algorithm to RANDOMIZED." << std::endl; 86 | break; 87 | default: 88 | DPP_ASSERT(0 && "Unknown planning algorithm"); 89 | Logger::logError() << "Unknown planning algorithm: " << algId << std::endl; 90 | return; 91 | } // switch (algId) 92 | } 93 | 94 | 95 | } // namespace dpp -------------------------------------------------------------------------------- /src/dpp/planner/PathPlanner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Implementation of DPP::PathPlanner abstract class. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | 10 | #include 11 | 12 | namespace dpp { 13 | 14 | PathPlanner::~PathPlanner() { 15 | } 16 | 17 | /** 18 | * Copy the solution (Graph, Tour, Edges, and Headings) into those given. 19 | * @note Existing nodes and edges are cleared. 20 | */ 21 | void DubinsPathPlanner::copySolution(ogdf::Graph &G, ogdf::GraphAttributes &GA, 22 | ogdf::List &Tour, ogdf::List &Edges, 23 | NodeArray &Headings, double &cost) { 24 | DPP_ASSERT(m_haveSolution); 25 | 26 | // Copy the graph and attributes 27 | NodeArray nodeCopyTable(m_G); 28 | EdgeArray edgeCopyTable(m_G); 29 | int n = copyGraph(m_G, m_GA, G, GA, nodeCopyTable, edgeCopyTable); 30 | 31 | // Copy the tour 32 | ogdf::ListIterator tourIter; 33 | for ( tourIter = m_Tour.begin(); tourIter != m_Tour.end(); tourIter++ ) { 34 | node u = *tourIter; 35 | node ucopy = nodeCopyTable(u); 36 | Tour.pushBack(ucopy); 37 | } 38 | 39 | // Copy the edge list 40 | ogdf::ListIterator edgeIter; 41 | for ( edgeIter = m_Edges.begin(); edgeIter != m_Edges.end(); edgeIter++ ) { 42 | edge e = *edgeIter; 43 | edge ecopy = edgeCopyTable(e); 44 | Edges.pushBack(ecopy); 45 | } 46 | 47 | // Copy the headings 48 | Headings.init(G); 49 | node u; 50 | forall_nodes(u,m_G) { 51 | node ucopy = nodeCopyTable(u); 52 | Headings(ucopy) = m_Headings(u); 53 | } 54 | 55 | // Copy the cost 56 | cost = m_cost; 57 | 58 | Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Copied " << n << " nodes from graph " << &m_G << " with " << waypointCount() 59 | << " nodes into graph " << &G << std::endl; 60 | } 61 | 62 | DubinsPathPlanner::~DubinsPathPlanner() { 63 | } 64 | 65 | } // namespace dpp -------------------------------------------------------------------------------- /src/mex/mexHelper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MATLAB MEX helper functions. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | #include "mexHelper.h" 10 | 11 | 12 | /** 13 | * Pack the graphs nodes into the matrix of vertices V. 14 | * @param G The graph to pack into V. 15 | * @param GA Attributes of the graph. 16 | * @param pV A pointer to an n-by-2 matrix of vertices. 17 | * @return Number of nodes packed into V. 18 | * @note pV should be allocated as n-by-2, where n is the number of nodes in G. 19 | */ 20 | int packNodes(Graph &G, GraphAttributes &GA, double *pV) { 21 | node u; 22 | int i = 0; 23 | int n = G.numberOfNodes(); 24 | forall_nodes(u,G) { 25 | MEX_MAT(pV, i, 0, n) = GA.x(u); 26 | MEX_MAT(pV, i, 1, n) = GA.y(u); 27 | i++; 28 | } 29 | 30 | return i; 31 | } 32 | 33 | /** 34 | * Adds vertices to the polygon by unpacking P. 35 | * @param polygon Polygon to add points to. 36 | * @param pP A pointer to an n-by-2 matrix of polygon vertices. 37 | * @param n Number of rows in pP. 38 | * @return Number of points added to the polygon. 39 | */ 40 | int unpackPolygon(DPolygon &polygon, double *pP, int n) { 41 | 42 | #ifdef MEX_DEBUG 43 | mexPrintf("Printing polygon vertices...\n"); 44 | #endif 45 | int i; 46 | for (i = 0; i < n; i++) { 47 | double x = MEX_MAT(pP, i, 0, n); 48 | double y = MEX_MAT(pP, i, 1, n); 49 | 50 | #ifdef MEX_DEBUG 51 | mexPrintf("Vertex %d at (%0.1f, %0.1f)\n", i + 1, x, y); 52 | #endif 53 | 54 | DPoint p(x,y); 55 | polygon.pushBack(p); 56 | } 57 | 58 | return i; 59 | } 60 | 61 | /** 62 | * Add nodes to the graph by unpacking V. 63 | * @param G Graph. 64 | * @param GA Attributes for the graph. 65 | * @param pV A pointer to an n-by-2 matrix of vertices as input. 66 | * @param n Number of rows in V. 67 | * @return Number of nodes added to the graph. 68 | */ 69 | int unpackNodes(Graph &G, GraphAttributes &GA, double *pV, int n) { 70 | 71 | #ifdef MEX_DEBUG 72 | mexPrintf("Building a graph with %d nodes.\n", n); 73 | mexEvalString("drawnow"); 74 | #endif 75 | 76 | int nodesCreated = 0; 77 | for (int i=0; i < n; i++) { 78 | node u = G.newNode(); 79 | GA.x(u) = MEX_MAT(pV,i,0,n); 80 | GA.y(u) = MEX_MAT(pV,i,1,n); 81 | GA.idNode(u) = nodesCreated+1; 82 | nodesCreated++; 83 | } 84 | 85 | return nodesCreated; 86 | } 87 | 88 | /** 89 | * Pack Edges into a m-by-3 matrix. 90 | * @param G Graph. 91 | * @param GA Attributes for the graph. 92 | * @param Edges An m-by-3 matrix of edges. 93 | * @param pE Pointer to m-by-3- matrix of edges for output. 94 | * @return Number of edges added. 95 | */ 96 | int packEdges(Graph &G, GraphAttributes &GA, List &Edges, double *pE) { 97 | int m = Edges.size(); 98 | #ifdef MEX_DEBUG 99 | mexPrintf("Printing tour...\n"); 100 | #endif 101 | int row = 0; 102 | edge e; 103 | forall_edges(e, G) { 104 | node u = e->source(); 105 | node v = e->target(); 106 | int u_id = GA.idNode(u); 107 | int v_id = GA.idNode(v); 108 | 109 | #ifdef MEX_DEBUG 110 | mexPrintf("Edge is 0X%X with weight %0.1f.\n", e, GA.doubleWeight(e)); 111 | mexPrintf("Node %d to %d.\n", u_id, v_id); 112 | #endif 113 | 114 | MEX_MAT(pE,row,0,m) = u_id; 115 | MEX_MAT(pE,row,1,m) = v_id; 116 | MEX_MAT(pE,row,2,m) = GA.doubleWeight(e); 117 | row++; 118 | } 119 | 120 | return row; 121 | } 122 | 123 | /** 124 | * Pack Headings into an n-by-1 vector. 125 | * @param G Graph. 126 | * @param GA Attributes for the graph. 127 | * @param X An n-by-1 vector of node headings. 128 | * @param pX Pointer to n-by-1 vector of headings for output. 129 | * @return Number of headings added. 130 | */ 131 | int packHeadings(Graph &G, GraphAttributes &GA, NodeArray &X, double *pX) { 132 | // Copy headings 133 | int i = 0; 134 | node u; 135 | forall_nodes(u, G) { 136 | int u_idx = GA.idNode(u) - 1; 137 | pX[u_idx] = X[u]; 138 | i++; 139 | } 140 | return i; 141 | } 142 | -------------------------------------------------------------------------------- /src/mex/mexHelper.h: -------------------------------------------------------------------------------- 1 | #ifndef _MEX_HELPER_H_ 2 | #define _MEX_HELPER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "mex.h" 12 | 13 | using ogdf::node; 14 | using ogdf::edge; 15 | using ogdf::Graph; 16 | using ogdf::GraphAttributes; 17 | using ogdf::List; 18 | using ogdf::ListIterator; 19 | using ogdf::NodeArray; 20 | using ogdf::DPoint; 21 | using ogdf::DPolygon; 22 | 23 | #define MEX_MODULE_NAME "DubinsPathPlanner" 24 | 25 | #define MEX_MAT(a,i,j,m) a[(i)+(j)*m] 26 | 27 | #define MEX_DEBUG 28 | 29 | 30 | int unpackNodes(Graph &G, GraphAttributes &GA, double *pV, int n); 31 | 32 | int unpackPolygon(DPolygon &polygon, double *pP, int n); 33 | 34 | int packNodes(Graph &G, GraphAttributes &GA, double *pV); 35 | 36 | int packEdges(Graph &G, GraphAttributes &GA, List &Edges, double *pE); 37 | 38 | int packHeadings(Graph &G, GraphAttributes &GA, NodeArray &X, double *pX); 39 | 40 | 41 | #endif // _MEX_HELPER_H_ -------------------------------------------------------------------------------- /src/mex/solveCppAsDtsp_interface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MEX interface for the solveCppAsDtsp binary. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "mexHelper.h" 16 | #include "solveCppAsDtsp.h" 17 | 18 | #define LOG_FILENAME "solveCppAsDtsp.log" 19 | 20 | /* 21 | * MEX gateway function. The MEX file synopsis is as follows: 22 | * 23 | * solveCppAsDtsp P C r e [return] [algorithm] 24 | * 25 | * Parameters: 26 | * P Points of polygon, n-by-2 matrix of vertex positions 27 | * C Initial configuration, 3-by-1 vector of x, y, and heading [rad] 28 | * r Vehicle turn radius [m] 29 | * e Sensor coverage width [m] 30 | * [return] Optional argument whether to return to the starting node 31 | * [algorithm] Optional DTSP algorithm name. Options are "alternating", 32 | * "nearest", and "randomized". 33 | * Returns: 34 | * V Vertices of coverage graph, n-by-2 matrix with rows: x, y 35 | * E Edges of tour, m-by-3 matrix with rows: srcNodeId, targNodeId, cost 36 | * X Headings of tour, n-by-1 vector of node headings 37 | * cost Total cost of tour 38 | */ 39 | void mexFunction( int nlhs, mxArray *plhs[], 40 | int nrhs, const mxArray *prhs[]) 41 | { 42 | // Enable logging 43 | #ifdef MEX_DEBUG 44 | dpp::Logger *log = dpp::Logger::Instance(); 45 | log->level(dpp::Logger::Level::LL_DEBUG); 46 | log->verbose(2); 47 | dpp::Logger::initializeLogger(LOG_FILENAME); 48 | mexPrintf("Logging to " LOG_FILENAME ".\n"); 49 | #endif 50 | 51 | /* check for proper number of arguments */ 52 | if(nrhs<4) { 53 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":path:nrhs", 54 | "At least four inputs are required."); 55 | } 56 | if(nrhs>6) { 57 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":path:nrhs", 58 | "Too many input arguments given."); 59 | } 60 | 61 | if(nlhs!=4) { 62 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":path:nlhs", 63 | "Four outputs required."); 64 | } 65 | 66 | // check that columns in P is 2 67 | if(mxGetN(prhs[0])!=2) { 68 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":P:wrongColumns", 69 | "P must have 2 columns."); 70 | } 71 | 72 | // check that rows in P is larger than 3 73 | if(mxGetM(prhs[0])<3) { 74 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":P:notEnoughRows", 75 | "P must have at least 3 rows."); 76 | } 77 | 78 | // check that C is 3-by-1 or 1-by-3 79 | //if(!((mxGetN(prhs[1]) == 3 && mxGetM(prhs[1]) == 1) 80 | // || (mxGetN(prhs[1]) == 1 && mxGetM(prhs[1]) == 3))) { 81 | if (mxGetNumberOfElements(prhs[1])!=3 ) { 82 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":C:wrongDimensions", 83 | "C must be 3-by-1 or 1-by-3."); 84 | } 85 | 86 | // TODO check that heading in C is between [0, 2*pi) 87 | 88 | // Ensure r and e are scalar Doubles 89 | if( !mxIsDouble(prhs[2]) || 90 | mxIsComplex(prhs[2]) || 91 | mxGetNumberOfElements(prhs[2])!=1 ) { 92 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":r:notScalar", 93 | "Input turn radius must be a scalar."); 94 | } 95 | 96 | if( !mxIsDouble(prhs[3]) || 97 | mxIsComplex(prhs[3]) || 98 | mxGetNumberOfElements(prhs[3])!=1 ) { 99 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":e:notScalar", 100 | "Input sensor width must be a scalar."); 101 | } 102 | 103 | size_t nP = mxGetM(prhs[0]); 104 | 105 | // Input variables 106 | double *P, *C, r, e; // input variables 107 | bool returnToInitial = false; 108 | std::string algorithmName("alternating"); 109 | 110 | P = mxGetPr(prhs[0]); 111 | C = mxGetPr(prhs[1]); 112 | r = mxGetScalar(prhs[2]); 113 | e = mxGetScalar(prhs[3]); 114 | 115 | // Handle the extra arguments (returnToInitial) 116 | if (nrhs >= 5) { 117 | returnToInitial = (bool)mxGetScalar(prhs[4]); 118 | } 119 | // ... extra args (algorithm) 120 | if (nrhs >= 6) { 121 | char buf[100]; 122 | if (mxGetString(prhs[5], buf, 100) != 0) { 123 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":algorithm:parsingName", 124 | "Failed reading algorithm name."); 125 | } 126 | algorithmName = buf; 127 | } 128 | 129 | // Create polygon object from vertices 130 | DPolygon polygon; 131 | unpackPolygon(polygon, P, nP); 132 | #ifdef MEX_DEBUG 133 | mexPrintf("Created polygon with %d points.\n", polygon.size()); 134 | #endif 135 | 136 | // Create initial vehicle config 137 | dpp::VehicleConfiguration VC(C[0], C[1], C[2]); 138 | #ifdef MEX_DEBUG 139 | dpp::Logger::logDebug(DPP_LOGGER_VERBOSE_1) << "Initial position (" 140 | << VC.x() << ", " << VC.y() << ") with heading: " << VC.heading() << std::endl; 141 | #endif 142 | 143 | // Set algorithm 144 | dpp::DtspPlanningAlgorithm alg; 145 | if (algorithmName.compare("alternating") == 0) { 146 | alg = dpp::DtspPlanningAlgorithm::ALTERNATING; 147 | } 148 | else if (algorithmName.compare("nearest") == 0) { 149 | alg = dpp::DtspPlanningAlgorithm::NEAREST_NEIGHBOR; 150 | } 151 | else if (algorithmName.compare("randomized") == 0) { 152 | alg = dpp::DtspPlanningAlgorithm::RANDOMIZED; 153 | } 154 | else { 155 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":algorithm:unknownName", 156 | "Unknown algorithm name given."); 157 | } 158 | 159 | // Call solver 160 | Graph G; 161 | GraphAttributes GA(G, DPP_GRAPH_ATTRIBUTES); 162 | List Tour; 163 | List Edges; 164 | NodeArray Headings(G,0.0); 165 | double cost; 166 | int result = solveCppAsDtsp(polygon, VC, G, GA, r, e, Tour, Edges, Headings, cost, returnToInitial, 167 | alg); 168 | 169 | if (result != SUCCESS) { 170 | char buf[50]; 171 | sprintf(buf, "Solver failed with code: %d", result); 172 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":solver:failure", buf); 173 | } 174 | int n = G.numberOfNodes(); 175 | int m = Edges.size(); 176 | 177 | // Output variables 178 | double *pV, *pE, *pX; 179 | plhs[0] = mxCreateDoubleMatrix((mwSize)n,(mwSize)2,mxREAL); 180 | plhs[1] = mxCreateDoubleMatrix((mwSize)m,(mwSize)3,mxREAL); 181 | plhs[2] = mxCreateDoubleMatrix((mwSize)n,(mwSize)1,mxREAL); 182 | plhs[3] = mxCreateDoubleScalar(cost); 183 | pV = mxGetPr(plhs[0]); 184 | pE = mxGetPr(plhs[1]); 185 | pX = mxGetPr(plhs[2]); 186 | 187 | // Pack outputs 188 | #ifdef MEX_DEBUG 189 | mexPrintf("Creating output tour edges m=%d, and headings n=%d\n",m,n); 190 | mexEvalString("drawnow"); 191 | #endif 192 | 193 | packNodes(G, GA, pV); 194 | 195 | packEdges(G, GA, Edges, pE); 196 | 197 | packHeadings(G, GA, Headings, pX); 198 | 199 | } -------------------------------------------------------------------------------- /src/mex/solveDtsp_interface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MEX interface for the solveDtsp binary. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "mexHelper.h" 16 | #include "solveDtsp.h" 17 | 18 | #define LOG_FILENAME "solveDtsp.log" 19 | 20 | /* 21 | * MEX gateway function. The MEX file synopsis is as follows: 22 | * 23 | * solveDtsp V x r [return] [algorithm] 24 | * 25 | * Parameters: 26 | * V Vertices, n-by-2 matrix of node positions 27 | * x Initial heading [rad] 28 | * r Vehicle turn radius [m] 29 | * [return] Optional argument whether to return to the starting node 30 | * [algorithm] Optional algorithm name. Options are "alternating", 31 | * "nearest", and "randomized". 32 | * Returns: 33 | * E Edges of tour, m-by-3 matrix with rows as: srcNodeId, targNodeId, cost 34 | * X Headings of tour, n-by-1 vector of node headings 35 | * cost Total cost of tour 36 | */ 37 | void mexFunction( int nlhs, mxArray *plhs[], 38 | int nrhs, const mxArray *prhs[]) 39 | { 40 | // Enable logging 41 | #ifdef MEX_DEBUG 42 | dpp::Logger *log = dpp::Logger::Instance(); 43 | log->level(dpp::Logger::Level::LL_DEBUG); 44 | log->verbose(2); 45 | dpp::Logger::initializeLogger(LOG_FILENAME); 46 | #endif 47 | 48 | /* check for proper number of arguments */ 49 | if(nrhs<3) { 50 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":path:nrhs", 51 | "At least three inputs are required."); 52 | } 53 | if(nrhs>5) { 54 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":path:nrhs", 55 | "Too many input arguments given."); 56 | } 57 | 58 | if(nlhs!=3) { 59 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":path:nlhs", 60 | "Three outputs required."); 61 | } 62 | 63 | // check that columns in V is 2 64 | if(mxGetN(prhs[0])!=2) { 65 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":V:wrongColumns", 66 | "V must have 2 columns."); 67 | } 68 | 69 | // check that rows in V is larger than 1 70 | if(mxGetM(prhs[0])<2) { 71 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":V:notEnoughRows", 72 | "V must have at least 2 rows."); 73 | } 74 | 75 | // Ensure x and r are scalar Doubles 76 | if( !mxIsDouble(prhs[1]) || 77 | mxIsComplex(prhs[1]) || 78 | mxGetNumberOfElements(prhs[1])!=1 ) { 79 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":x:notScalar", 80 | "Input heading must be a scalar."); 81 | } 82 | 83 | if( !mxIsDouble(prhs[2]) || 84 | mxIsComplex(prhs[2]) || 85 | mxGetNumberOfElements(prhs[2])!=1 ) { 86 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":r:notScalar", 87 | "Input radius must be a scalar."); 88 | } 89 | 90 | size_t n = mxGetM(prhs[0]); 91 | size_t cols = mxGetN(prhs[0]); 92 | 93 | // Input variables 94 | double *V, x, r; // input variables 95 | bool returnToInitial = false; 96 | std::string algorithmName("alternating"); 97 | 98 | // Output variables 99 | double *E, *X, cost; // output variables 100 | 101 | V = mxGetPr(prhs[0]); 102 | x = mxGetScalar(prhs[1]); 103 | r = mxGetScalar(prhs[2]); 104 | 105 | if (nrhs >= 4) { 106 | returnToInitial = (bool)mxGetScalar(prhs[3]); 107 | } 108 | if (nrhs >= 5) { 109 | char buf[100]; 110 | if (mxGetString(prhs[4], buf, 100) != 0) { 111 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":algorithm:parsingName", 112 | "Failed reading algorithm name."); 113 | } 114 | algorithmName = buf; 115 | } 116 | 117 | // Construct the graph 118 | Graph G; 119 | GraphAttributes GA(G, DPP_GRAPH_ATTRIBUTES); 120 | unpackNodes(G, GA, V, n); 121 | node u; 122 | 123 | #ifdef MEX_DEBUG 124 | mexPrintf("Printing graph...\n"); 125 | forall_nodes(u, G) { 126 | mexPrintf("Node %d at (%0.1f, %0.1f)\n",GA.idNode(u),GA.x(u),GA.y(u)); 127 | } 128 | mexPrintf("\n"); 129 | 130 | dpp::Logger::logDebug(DPP_LOGGER_VERBOSE_3) << dpp::printGraph(G, GA); 131 | #endif 132 | 133 | // Set algorithm 134 | dpp::DtspPlanningAlgorithm alg; 135 | if (algorithmName.compare("alternating") == 0) { 136 | alg = dpp::DtspPlanningAlgorithm::ALTERNATING; 137 | } 138 | else if (algorithmName.compare("nearest") == 0) { 139 | alg = dpp::DtspPlanningAlgorithm::NEAREST_NEIGHBOR; 140 | } 141 | else if (algorithmName.compare("randomized") == 0) { 142 | alg = dpp::DtspPlanningAlgorithm::RANDOMIZED; 143 | } 144 | else { 145 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":algorithm:unknownName", 146 | "Unknown algorithm name given."); 147 | } 148 | 149 | // Call solver 150 | List Tour; 151 | List Edges; 152 | NodeArray Headings(G,0.0); 153 | int result = solveDtsp(G, GA, x, r, Tour, Edges, Headings, cost, returnToInitial, 154 | alg); 155 | 156 | if (result != SUCCESS) { 157 | char buf[50]; 158 | sprintf(buf, "Solver failed with code: %d", result); 159 | mexErrMsgIdAndTxt(MEX_MODULE_NAME ":solver:failure", buf); 160 | } 161 | int m = Edges.size(); 162 | 163 | // Build output matrices 164 | plhs[0] = mxCreateDoubleMatrix((mwSize)m,(mwSize)3,mxREAL); 165 | plhs[1] = mxCreateDoubleMatrix((mwSize)n,(mwSize)1,mxREAL); 166 | plhs[2] = mxCreateDoubleScalar(cost); 167 | double *pE = mxGetPr(plhs[0]); 168 | double *pX = mxGetPr(plhs[1]); 169 | 170 | // Pack outputs 171 | #ifdef MEX_DEBUG 172 | mexPrintf("Creating output tour edges m=%d, and headings n=%d\n",m,n); 173 | mexEvalString("drawnow"); 174 | #endif 175 | 176 | packEdges(G, GA, Edges, pE); 177 | 178 | packHeadings(G, GA, Headings, pX); 179 | } -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists 2 | 3 | ################################ 4 | # GTest 5 | ################################ 6 | # googletest & googlemock 7 | set(GMOCK_DIR "gtest/googlemock" 8 | CACHE PATH "The path to the GoogleMock test framework.") 9 | 10 | if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") 11 | # force this option to ON so that Google Test will use /MD instead of /MT 12 | # /MD is now the default for Visual Studio, so it should be our default, too 13 | option(gtest_force_shared_crt 14 | "Use shared (DLL) run-time lib even when Google Test is built as static lib." 15 | ON) 16 | elseif (APPLE) 17 | add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=1) 18 | endif() 19 | 20 | add_subdirectory(${GMOCK_DIR} ${CMAKE_BINARY_DIR}/gmock) 21 | include_directories(SYSTEM ${GMOCK_DIR}/../googletest/include 22 | ${GMOCK_DIR}/include) 23 | 24 | 25 | ################################ 26 | # Extra Dependencies 27 | ################################ 28 | # DubinsCurves 29 | #link_directories( ${CMAKE_SOURCE_DIR}/lib/dubins-curves ) 30 | #include_directories( ${CMAKE_SOURCE_DIR}/lib/dubins-curves/include ) 31 | 32 | # DPP 33 | include_directories( ${CMAKE_SOURCE_DIR}/test ) 34 | 35 | ################################ 36 | # Unit Tests 37 | ################################ 38 | 39 | # dpp/basic/Logger 40 | add_executable(Logger_test basic/Logger_test.cpp) 41 | target_link_libraries(Logger_test gmock_main DPP) 42 | add_test(BasicTests Logger_test) 43 | 44 | # dpp/basic/Util 45 | add_executable(Util_test basic/Util_test.cpp ) 46 | target_link_libraries(Util_test gmock_main OGDF COIN DPP ) 47 | add_test(BasicTests Util_test) 48 | 49 | # dpp/basic/Path 50 | add_executable(Path_test basic/Path_test.cpp) 51 | target_link_libraries(Path_test gmock_main OGDF COIN DPP DUBINSCURVES) 52 | add_test(BasicTests Path_test) 53 | 54 | # dpp/planalg/Field 55 | add_executable(Field_test basic/Field_test.cpp) 56 | target_link_libraries(Field_test gmock_main OGDF COIN DPP) 57 | add_test(PlanalgTests Field_test) 58 | 59 | # dpp/planalg/RandomizedDtsp 60 | add_executable(RandomizedDtsp_test planalg/RandomizedDtsp_test.cpp) 61 | target_link_libraries(RandomizedDtsp_test gmock_main OGDF COIN DPP) 62 | add_test(PlanalgTests RandomizedDtsp_test) 63 | 64 | # dpp/planalg/AlternatingDtsp 65 | add_executable(AlternatingDtsp_test planalg/AlternatingDtsp_test.cpp) 66 | target_link_libraries(AlternatingDtsp_test gmock_main OGDF COIN DPP) 67 | add_test(PlanalgTests AlternatingDtsp_test) 68 | 69 | # dpp/planalg/NearestNeighborDtsp 70 | add_executable(NearestNeighborDtsp_test planalg/NearestNeighborDtsp_test.cpp) 71 | target_link_libraries(NearestNeighborDtsp_test gmock_main OGDF COIN DPP) 72 | add_test(PlanalgTests NearestNeighborDtsp_test) 73 | 74 | # dpp/planner/DubinsVehiclePathPlanner 75 | add_executable(DubinsVehiclePathPlanner_test planner/DubinsVehiclePathPlanner_test.cpp) 76 | target_link_libraries(DubinsVehiclePathPlanner_test gmock_main OGDF COIN DPP) 77 | add_test(PlannerTests DubinsVehiclePathPlanner_test) 78 | 79 | # dpp/planner/WaypointSequencePlanner 80 | add_executable(WaypointSequencePlanner_test planner/WaypointSequencePlanner_test.cpp) 81 | target_link_libraries(WaypointSequencePlanner_test gmock_main OGDF COIN DPP) 82 | add_test(PlannerTests WaypointSequencePlanner_test) 83 | 84 | # dpp/planner/CoverageWaypointPlanner 85 | add_executable(CoverageWaypointPlanner_test planner/CoverageWaypointPlanner_test.cpp) 86 | target_link_libraries(CoverageWaypointPlanner_test gmock_main OGDF COIN DPP) 87 | add_test(PlannerTests CoverageWaypointPlanner_test) -------------------------------------------------------------------------------- /test/DubinsPathPlanner_test.h: -------------------------------------------------------------------------------- 1 | #ifndef _DUBINS_PATH_PLANNER_TEST_H_ 2 | #define _DUBINS_PATH_PLANNER_TEST_H_ 3 | 4 | #include 5 | #include 6 | 7 | #define ENABLE_DEBUG_TEST() do {\ 8 | dpp::Logger *log = dpp::Logger::Instance(); \ 9 | log->level(DPP_LOGGER_LEVEL_DEBUG); \ 10 | log->verbose(DPP_LOGGER_VERBOSE_2); \ 11 | } while(0) 12 | 13 | #define DISABLE_DEBUG_TEST() do {\ 14 | dpp::Logger *log = dpp::Logger::Instance(); \ 15 | log->verbose(DPP_LOGGER_DEFAULT_VERBOSE); \ 16 | log->level(DPP_LOGGER_DEFAULT_LEVEL); \ 17 | } while(0) 18 | 19 | // TODO add debug wrapper macro for tests? (DTEST_F, DTEST, DTEST_P) 20 | 21 | #endif // _DUBINS_PATH_PLANNER_TEST_H_ -------------------------------------------------------------------------------- /test/basic/FileIO_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Unit test for the dpp/basic/FileIO.h. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | // TODO add this test 10 | -------------------------------------------------------------------------------- /test/planalg/AlternatingDtsp_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014-2015 DubinsPathPlanner. 3 | * Created by David Goodman 4 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 5 | * For details see the LICENSE file distributed with DubinsPathPlanner. 6 | */ 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | using ogdf::node; 18 | using ogdf::edge; 19 | using ogdf::Graph; 20 | using ogdf::GraphAttributes; 21 | using ogdf::DPoint; 22 | using ogdf::List; 23 | using ogdf::ListIterator; 24 | using ogdf::ListConstIterator; 25 | using ogdf::NodeArray; 26 | 27 | #define TURN_RADIUS 1.0 28 | #define EPSILON_ERROR 0.0001 // error margin in length calculation 29 | 30 | 31 | // Value-parameterized tests for dubinsTourCost() 32 | //#if GTEST_HAS_PARAM_TEST 33 | using ::testing::Test; 34 | //using ::testing::TestWithParam; 35 | using ::testing::Values; 36 | using ::testing::ValuesIn; 37 | 38 | const List nodes { 39 | {0, 0}, 40 | {5.1*TURN_RADIUS, 0.0}, 41 | {5.1*TURN_RADIUS, -3.3*TURN_RADIUS}, 42 | {0, -6.6*TURN_RADIUS}, 43 | {5.1*TURN_RADIUS, -6.6*TURN_RADIUS} 44 | }; 45 | 46 | class AlternatingDtspTest : public Test { 47 | public: 48 | AlternatingDtspTest() 49 | : m_G(), 50 | m_GA (m_G, 51 | GraphAttributes::nodeGraphics | 52 | GraphAttributes::edgeGraphics | 53 | GraphAttributes::nodeLabel | 54 | GraphAttributes::edgeStyle | 55 | GraphAttributes::edgeDoubleWeight | 56 | GraphAttributes::nodeStyle | 57 | GraphAttributes::nodeTemplate | 58 | GraphAttributes::nodeId), 59 | m_Headings(m_G), 60 | m_turnRadius(TURN_RADIUS), 61 | m_initialHeading(0.0), 62 | m_alg() 63 | { } 64 | 65 | virtual ~AlternatingDtspTest() { } 66 | 67 | virtual void SetUp() { 68 | int m = nodes.size(); 69 | 70 | // Build graph 71 | ListConstIterator iter; 72 | int i = 1; 73 | for ( iter = nodes.begin(); iter != nodes.end(); iter++ ) { 74 | DPoint Pu = *iter; 75 | node u = m_G.newNode(); 76 | m_GA.x(u) = Pu.m_x; 77 | m_GA.y(u) = Pu.m_y; 78 | m_GA.idNode(u) = i++; 79 | } 80 | 81 | } // InitializeScenario() 82 | 83 | virtual void TearDown() { 84 | } // TearDown() 85 | 86 | int GetSize() { 87 | return m_G.numberOfNodes(); 88 | } 89 | 90 | protected: 91 | Graph m_G; 92 | GraphAttributes m_GA; 93 | NodeArray m_Headings; 94 | List m_Tour; 95 | List m_Edges; 96 | double m_cost; 97 | const double m_turnRadius; 98 | double m_initialHeading; 99 | dpp::AlternatingDtsp m_alg; 100 | 101 | }; // class AlternatingDtspTest 102 | 103 | // Test for out of range error 104 | TEST_F(AlternatingDtspTest, InitialHeadingOutOfRangeError) { 105 | double x = -1.1; // initial heading 106 | 107 | EXPECT_THROW(m_alg.run(m_G, m_GA, x, m_turnRadius, m_Tour, m_Edges, 108 | m_Headings, m_cost), std::out_of_range); 109 | 110 | x = 2*M_PI; // initial heading 111 | 112 | EXPECT_THROW(m_alg.run(m_G, m_GA, x, m_turnRadius, m_Tour, m_Edges, 113 | m_Headings, m_cost), std::out_of_range); 114 | } 115 | 116 | // Test for headings domain error 117 | TEST_F(AlternatingDtspTest, HeadingsDomainError) { 118 | Graph G2; 119 | m_Headings.init(G2); 120 | EXPECT_THROW(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 121 | m_Headings, m_cost), std::domain_error); 122 | } 123 | 124 | // Test for not enough nodes error 125 | TEST_F(AlternatingDtspTest, NodesOutOfRangeError) { 126 | node u; 127 | // This doesn't work? 128 | //std::cout << "Graph: " << &m_G << std::endl; 129 | //forall_nodes(u,m_G) { 130 | // std::cout << "Node " << m_GA.idNode(u) << " of graph: " << u->graphOf() << std::endl; 131 | // m_G.delNode(u); 132 | //} 133 | m_G.clear(); 134 | 135 | EXPECT_THROW(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 136 | m_Headings, m_cost), std::out_of_range); 137 | } 138 | 139 | // Test for assertion fails with existing edges 140 | TEST_F(AlternatingDtspTest, ExistingEdgesFails) { 141 | // shouldn't need to add this to m_Edges 142 | edge e = m_G.newEdge(m_G.firstNode(), m_G.lastNode()); 143 | 144 | EXPECT_DEATH(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 145 | m_Headings, m_cost), "Assertion failed: \\(G\\.numberOfEdges\\(\\) < 1\\).*"); 146 | } 147 | 148 | // Test for working tour with no return 149 | TEST_F(AlternatingDtspTest, TourWithNoReturn) { 150 | double expectedCost = 19.447501521580278; 151 | int expectedEdgesSize = GetSize() - 1; 152 | int expectedTourSize = GetSize(); 153 | 154 | EXPECT_EQ(SUCCESS, m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 155 | m_Headings, m_cost, false)); 156 | EXPECT_EQ(expectedTourSize, m_Tour.size()); 157 | EXPECT_EQ(expectedEdgesSize, m_Edges.size()); 158 | EXPECT_NEAR(expectedCost, m_cost, EPSILON_ERROR); 159 | } 160 | 161 | // Test for working tour with return 162 | TEST_F(AlternatingDtspTest, TourWithReturn) { 163 | double expectedCost = 26.04750152158028; 164 | int expectedEdgesSize = GetSize(); 165 | int expectedTourSize = GetSize() + 1; 166 | 167 | dpp::Logger *log = dpp::Logger::Instance(); 168 | log->level(dpp::Logger::Level::LL_DEBUG); 169 | log->verbose(0); 170 | 171 | EXPECT_EQ(SUCCESS, m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 172 | m_Headings, m_cost, true)); 173 | EXPECT_EQ(expectedTourSize, m_Tour.size()); 174 | EXPECT_EQ(expectedEdgesSize, m_Edges.size()); 175 | EXPECT_NEAR(expectedCost, m_cost, EPSILON_ERROR); 176 | } 177 | -------------------------------------------------------------------------------- /test/planalg/NearestNeighborDtsp_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014-2015 DubinsPathPlanner. 3 | * Created by David Goodman 4 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 5 | * For details see the LICENSE file distributed with DubinsPathPlanner. 6 | */ 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | using ogdf::node; 18 | using ogdf::edge; 19 | using ogdf::Graph; 20 | using ogdf::GraphAttributes; 21 | using ogdf::DPoint; 22 | using ogdf::List; 23 | using ogdf::ListIterator; 24 | using ogdf::ListConstIterator; 25 | using ogdf::NodeArray; 26 | 27 | #define TURN_RADIUS 1.0 28 | #define EPSILON_ERROR 0.0001 // error margin in length calculation 29 | 30 | 31 | // Value-parameterized tests for dubinsTourCost() 32 | //#if GTEST_HAS_PARAM_TEST 33 | using ::testing::Test; 34 | //using ::testing::TestWithParam; 35 | using ::testing::Values; 36 | using ::testing::ValuesIn; 37 | 38 | const List nodes { 39 | {0, 0}, 40 | {5.1*TURN_RADIUS, 0.0}, 41 | {5.1*TURN_RADIUS, -3.3*TURN_RADIUS}, 42 | {0, -6.6*TURN_RADIUS}, 43 | {5.1*TURN_RADIUS, -6.6*TURN_RADIUS} 44 | }; 45 | 46 | class NearestNeighborDtspTest : public Test { 47 | public: 48 | NearestNeighborDtspTest() 49 | : m_G(), 50 | m_GA (m_G, 51 | GraphAttributes::nodeGraphics | 52 | GraphAttributes::edgeGraphics | 53 | GraphAttributes::nodeLabel | 54 | GraphAttributes::edgeStyle | 55 | GraphAttributes::edgeDoubleWeight | 56 | GraphAttributes::nodeStyle | 57 | GraphAttributes::nodeTemplate | 58 | GraphAttributes::nodeId), 59 | m_Headings(m_G), 60 | m_turnRadius(TURN_RADIUS), 61 | m_initialHeading(0.0), 62 | m_alg() 63 | { } 64 | 65 | virtual ~NearestNeighborDtspTest() { } 66 | 67 | virtual void SetUp() { 68 | int m = nodes.size(); 69 | 70 | // Build graph 71 | ListConstIterator iter; 72 | int i = 1; 73 | for ( iter = nodes.begin(); iter != nodes.end(); iter++ ) { 74 | DPoint Pu = *iter; 75 | node u = m_G.newNode(); 76 | m_GA.x(u) = Pu.m_x; 77 | m_GA.y(u) = Pu.m_y; 78 | m_GA.idNode(u) = i++; 79 | } 80 | 81 | } // InitializeScenario() 82 | 83 | virtual void TearDown() { 84 | } // TearDown() 85 | 86 | int GetSize() { 87 | return m_G.numberOfNodes(); 88 | } 89 | 90 | protected: 91 | Graph m_G; 92 | GraphAttributes m_GA; 93 | NodeArray m_Headings; 94 | List m_Tour; 95 | List m_Edges; 96 | double m_cost; 97 | const double m_turnRadius; 98 | double m_initialHeading; 99 | dpp::NearestNeighborDtsp m_alg; 100 | 101 | }; // class NearestNeighborDtspTest 102 | 103 | // Test for out of range error 104 | TEST_F(NearestNeighborDtspTest, InitialHeadingOutOfRangeError) { 105 | double x = -1.1; // initial heading 106 | 107 | EXPECT_THROW(m_alg.run(m_G, m_GA, x, m_turnRadius, m_Tour, m_Edges, 108 | m_Headings, m_cost), std::out_of_range); 109 | 110 | x = 2*M_PI; // initial heading 111 | 112 | EXPECT_THROW(m_alg.run(m_G, m_GA, x, m_turnRadius, m_Tour, m_Edges, 113 | m_Headings, m_cost), std::out_of_range); 114 | } 115 | 116 | // Test for headings domain error 117 | TEST_F(NearestNeighborDtspTest, HeadingsDomainError) { 118 | Graph G2; 119 | m_Headings.init(G2); 120 | EXPECT_THROW(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 121 | m_Headings, m_cost), std::domain_error); 122 | } 123 | 124 | // Test for not enough nodes error 125 | TEST_F(NearestNeighborDtspTest, NodesOutOfRangeError) { 126 | node u; 127 | // This doesn't work? 128 | //std::cout << "Graph: " << &m_G << std::endl; 129 | //forall_nodes(u,m_G) { 130 | // std::cout << "Node " << m_GA.idNode(u) << " of graph: " << u->graphOf() << std::endl; 131 | // m_G.delNode(u); 132 | //} 133 | m_G.clear(); 134 | 135 | EXPECT_THROW(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 136 | m_Headings, m_cost), std::out_of_range); 137 | } 138 | 139 | // Test for assertion fails with existing edges 140 | TEST_F(NearestNeighborDtspTest, ExistingEdgesFails) { 141 | // shouldn't need to add this to m_Edges 142 | edge e = m_G.newEdge(m_G.firstNode(), m_G.lastNode()); 143 | 144 | EXPECT_THROW(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 145 | m_Headings, m_cost), std::out_of_range); 146 | } 147 | 148 | // Test for working tour with no return 149 | TEST_F(NearestNeighborDtspTest, TourWithNoReturn) { 150 | double expectedCost = 19.001879768789465; 151 | int expectedEdgesSize = GetSize() - 1; 152 | int expectedTourSize = GetSize(); 153 | 154 | EXPECT_EQ(SUCCESS, m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 155 | m_Headings, m_cost, false)); 156 | EXPECT_EQ(expectedTourSize, m_Tour.size()); 157 | EXPECT_EQ(expectedEdgesSize, m_Edges.size()); 158 | EXPECT_NEAR(expectedCost, m_cost, EPSILON_ERROR); 159 | } 160 | 161 | // Test for working tour with return 162 | TEST_F(NearestNeighborDtspTest, TourWithReturn) { 163 | double expectedCost = 26.263188532775452; 164 | int expectedEdgesSize = GetSize(); 165 | int expectedTourSize = GetSize() + 1; 166 | 167 | dpp::Logger *log = dpp::Logger::Instance(); 168 | log->level(dpp::Logger::Level::LL_DEBUG); 169 | log->verbose(0); 170 | 171 | EXPECT_EQ(SUCCESS, m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 172 | m_Headings, m_cost, true)); 173 | EXPECT_EQ(expectedTourSize, m_Tour.size()); 174 | EXPECT_EQ(expectedEdgesSize, m_Edges.size()); 175 | EXPECT_NEAR(expectedCost, m_cost, EPSILON_ERROR); 176 | } 177 | -------------------------------------------------------------------------------- /test/planalg/RandomizedDtsp_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014-2015 DubinsPathPlanner. 3 | * Created by David Goodman 4 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 5 | * For details see the LICENSE file distributed with DubinsPathPlanner. 6 | */ 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | using ogdf::node; 18 | using ogdf::edge; 19 | using ogdf::Graph; 20 | using ogdf::GraphAttributes; 21 | using ogdf::DPoint; 22 | using ogdf::List; 23 | using ogdf::ListIterator; 24 | using ogdf::ListConstIterator; 25 | using ogdf::NodeArray; 26 | 27 | #define TURN_RADIUS 1.0 28 | #define EPSILON_RANDOM_COST_ERROR (5.0*TURN_RADIUS) // stddev of solution cost 29 | 30 | 31 | // Value-parameterized tests for dubinsTourCost() 32 | //#if GTEST_HAS_PARAM_TEST 33 | using ::testing::Test; 34 | //using ::testing::TestWithParam; 35 | using ::testing::Values; 36 | using ::testing::ValuesIn; 37 | 38 | const List nodes { 39 | {0, 0}, 40 | {5.1*TURN_RADIUS, 0.0}, 41 | {5.1*TURN_RADIUS, -3.3*TURN_RADIUS}, 42 | {0, -6.6*TURN_RADIUS}, 43 | {5.1*TURN_RADIUS, -6.6*TURN_RADIUS} 44 | }; 45 | 46 | class RandomizedDtspTest : public Test { 47 | public: 48 | RandomizedDtspTest() 49 | : m_G(), 50 | m_GA (m_G, 51 | GraphAttributes::nodeGraphics | 52 | GraphAttributes::edgeGraphics | 53 | GraphAttributes::nodeLabel | 54 | GraphAttributes::edgeStyle | 55 | GraphAttributes::edgeDoubleWeight | 56 | GraphAttributes::nodeStyle | 57 | GraphAttributes::nodeTemplate | 58 | GraphAttributes::nodeId), 59 | m_Headings(m_G), 60 | m_turnRadius(TURN_RADIUS), 61 | m_initialHeading(0.0), 62 | m_alg() 63 | { } 64 | 65 | virtual ~RandomizedDtspTest() { } 66 | 67 | virtual void SetUp() { 68 | int m = nodes.size(); 69 | 70 | // Build graph 71 | ListConstIterator iter; 72 | int i = 1; 73 | for ( iter = nodes.begin(); iter != nodes.end(); iter++ ) { 74 | DPoint Pu = *iter; 75 | node u = m_G.newNode(); 76 | m_GA.x(u) = Pu.m_x; 77 | m_GA.y(u) = Pu.m_y; 78 | m_GA.idNode(u) = i++; 79 | } 80 | 81 | } // InitializeScenario() 82 | 83 | virtual void TearDown() { 84 | } // TearDown() 85 | 86 | int GetSize() { 87 | return m_G.numberOfNodes(); 88 | } 89 | 90 | protected: 91 | Graph m_G; 92 | GraphAttributes m_GA; 93 | NodeArray m_Headings; 94 | List m_Tour; 95 | List m_Edges; 96 | double m_cost; 97 | const double m_turnRadius; 98 | double m_initialHeading; 99 | dpp::RandomizedDtsp m_alg; 100 | 101 | }; // class RandomizedDtspTest 102 | 103 | // Test for out of range error 104 | TEST_F(RandomizedDtspTest, InitialHeadingOutOfRangeError) { 105 | double x = -1.1; // initial heading 106 | 107 | EXPECT_THROW(m_alg.run(m_G, m_GA, x, m_turnRadius, m_Tour, m_Edges, 108 | m_Headings, m_cost), std::out_of_range); 109 | 110 | x = 2*M_PI; // initial heading 111 | 112 | EXPECT_THROW(m_alg.run(m_G, m_GA, x, m_turnRadius, m_Tour, m_Edges, 113 | m_Headings, m_cost), std::out_of_range); 114 | } 115 | 116 | // Test for headings domain error 117 | TEST_F(RandomizedDtspTest, HeadingsDomainError) { 118 | Graph G2; 119 | m_Headings.init(G2); 120 | EXPECT_THROW(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 121 | m_Headings, m_cost), std::domain_error); 122 | } 123 | 124 | // Test for not enough nodes error 125 | TEST_F(RandomizedDtspTest, NodesOutOfRangeError) { 126 | node u; 127 | // This doesn't work? 128 | //std::cout << "Graph: " << &m_G << std::endl; 129 | //forall_nodes(u,m_G) { 130 | // std::cout << "Node " << m_GA.idNode(u) << " of graph: " << u->graphOf() << std::endl; 131 | // m_G.delNode(u); 132 | //} 133 | m_G.clear(); 134 | 135 | EXPECT_THROW(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 136 | m_Headings, m_cost), std::out_of_range); 137 | } 138 | 139 | // Test for assertion fails with existing edges 140 | TEST_F(RandomizedDtspTest, ExistingEdgesFails) { 141 | // shouldn't need to add this to m_Edges 142 | edge e = m_G.newEdge(m_G.firstNode(), m_G.lastNode()); 143 | 144 | EXPECT_DEATH(m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 145 | m_Headings, m_cost), "Assertion failed: \\(G\\.numberOfEdges\\(\\) < 1\\).*"); 146 | } 147 | 148 | // Test for working tour with no return 149 | TEST_F(RandomizedDtspTest, TourWithNoReturn) { 150 | double expectedCost = 20.15; 151 | int expectedEdgesSize = GetSize() - 1; 152 | int expectedTourSize = GetSize(); 153 | 154 | EXPECT_EQ(SUCCESS, m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 155 | m_Headings, m_cost, false)); 156 | EXPECT_EQ(expectedTourSize, m_Tour.size()); 157 | EXPECT_EQ(expectedEdgesSize, m_Edges.size()); 158 | EXPECT_NEAR(expectedCost, m_cost, EPSILON_RANDOM_COST_ERROR); 159 | } 160 | 161 | // Test for working tour with return 162 | TEST_F(RandomizedDtspTest, TourWithReturn) { 163 | double expectedCost = 29.85; 164 | int expectedEdgesSize = GetSize(); 165 | int expectedTourSize = GetSize() + 1; 166 | 167 | dpp::Logger *log = dpp::Logger::Instance(); 168 | log->level(dpp::Logger::Level::LL_DEBUG); 169 | log->verbose(0); 170 | 171 | EXPECT_EQ(SUCCESS, m_alg.run(m_G, m_GA, m_initialHeading, m_turnRadius, m_Tour, m_Edges, 172 | m_Headings, m_cost, true)); 173 | EXPECT_EQ(expectedTourSize, m_Tour.size()); 174 | EXPECT_EQ(expectedEdgesSize, m_Edges.size()); 175 | EXPECT_NEAR(expectedCost, m_cost, EPSILON_RANDOM_COST_ERROR); 176 | } 177 | -------------------------------------------------------------------------------- /test/planner/CoverageWaypointPlanner_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Unit test for dpp/planner/WaypointSequencePlanner. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | using std::vector; 19 | 20 | using dpp::VertexList; 21 | 22 | // Parameters to use 23 | #define TURN_RADIUS 5.0 24 | #define SENSOR_WIDTH 2.0 25 | #define INITIAL_HEADING 1.0 26 | #define PLANNING_ALGORITHM dpp::CppPlanningAlgorithm::BOUSTROPHEDON 27 | #define PLANNING_ALGORITHM_NAME "Boustrophedon" 28 | 29 | 30 | // Test initialiation is correct 31 | TEST (CoverageWaypointPlannerTest, ConstructsCorrectly) { 32 | // Verify initial state 33 | { 34 | dpp::CoverageWaypointPlanner *p = new dpp::CoverageWaypointPlanner(); 35 | 36 | EXPECT_FALSE(p->haveSolution()); 37 | EXPECT_EQ(0, p->waypointCount()); 38 | } 39 | 40 | // Turn radius specifiable 41 | { 42 | dpp::CoverageWaypointPlanner *p = new dpp::CoverageWaypointPlanner(TURN_RADIUS); 43 | EXPECT_EQ(TURN_RADIUS, p->turnRadius()); 44 | } 45 | 46 | // Sensor width specifiable 47 | { 48 | dpp::CoverageWaypointPlanner *p = new dpp::CoverageWaypointPlanner(TURN_RADIUS, SENSOR_WIDTH); 49 | EXPECT_EQ(SENSOR_WIDTH, p->sensorWidth()); 50 | } 51 | // Initial configuration specifiable 52 | { 53 | dpp::CoverageWaypointPlanner *p = new dpp::CoverageWaypointPlanner(TURN_RADIUS, SENSOR_WIDTH); 54 | dpp::VehicleConfiguration initialConfig(0, 0, INITIAL_HEADING); 55 | p->initialConfiguration(initialConfig); 56 | EXPECT_TRUE(initialConfig == p->initialConfiguration()); 57 | } 58 | // Algorithm specifiable 59 | { 60 | dpp::CoverageWaypointPlanner *p = new dpp::CoverageWaypointPlanner(TURN_RADIUS, SENSOR_WIDTH); 61 | p->algorithm(PLANNING_ALGORITHM); 62 | EXPECT_EQ(PLANNING_ALGORITHM_NAME, p->algorithmName()); 63 | } 64 | } 65 | 66 | // Test dies without solution 67 | TEST(CoverageWaypointPlannerTest, DeathWithoutSolution) { 68 | dpp::CoverageWaypointPlanner p; 69 | 70 | ASSERT_EQ(0, p.vertexCount()); 71 | ASSERT_FALSE(p.haveSolution()); 72 | 73 | dpp::WaypointList list; 74 | EXPECT_DEATH(list = p.waypointList(), "Assertion failed: \\(haveSolution\\(\\).*"); 75 | } 76 | 77 | // Test dies without polygon 78 | TEST(CoverageWaypointPlannerTest, DeathWithoutPolygon) { 79 | dpp::CoverageWaypointPlanner p; 80 | 81 | ASSERT_EQ(0, p.vertexCount()); 82 | ASSERT_FALSE(p.haveSolution()); 83 | 84 | EXPECT_DEATH(p.planCoverageWaypoints(), "Assertion failed: \\(vertexCount\\(\\) >= 3\\).*"); 85 | } 86 | 87 | // ------- Test data -------- 88 | // Convex polygon 89 | const VertexList rightTriangleVertices { 90 | {0, 0}, 91 | {-10.0, 10.0}, 92 | {-10.0, 0} 93 | }; 94 | 95 | // Non-convex polygon 96 | const VertexList nonConvexVertices { 97 | {0, 0}, 98 | {-10, 0}, 99 | {-10.0, -10.0}, 100 | {10, -10}, 101 | {10, -5}, 102 | {0, -5} 103 | }; 104 | 105 | 106 | // Test adding vertices 107 | TEST(CoverageWaypointPlannerTest, AddsVertices) { 108 | dpp::CoverageWaypointPlanner p; 109 | 110 | ASSERT_EQ(0, p.vertexCount()); 111 | ASSERT_FALSE(p.haveSolution()); 112 | 113 | int expectedN = rightTriangleVertices.size(); 114 | EXPECT_EQ(expectedN, p.addPolygonVertices(rightTriangleVertices)); 115 | EXPECT_EQ(expectedN, p.vertexCount()); 116 | } 117 | 118 | // Test dies with non-convex polygon 119 | TEST(CoverageWaypointPlannerTest, DeathWithNonConvexPolygon) { 120 | ENABLE_DEBUG_TEST(); 121 | dpp::CoverageWaypointPlanner p; 122 | 123 | ASSERT_EQ(0, p.vertexCount()); 124 | ASSERT_FALSE(p.haveSolution()); 125 | 126 | int expectedN = nonConvexVertices.size(); 127 | ASSERT_EQ(expectedN, p.addPolygonVertices(nonConvexVertices)); 128 | ASSERT_EQ(expectedN, p.vertexCount()); 129 | 130 | EXPECT_FALSE(p.planCoverageWaypoints()); 131 | } 132 | -------------------------------------------------------------------------------- /test/planner/DubinsSensorPathPlanner_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Unit test for dpp/planner/DubinsSensorPathPlanner. 3 | * 4 | * Copyright (C) 2014-2015 DubinsPathPlanner. 5 | * Created by David Goodman 6 | * Redistribution and use of this file is allowed according to the terms of the MIT license. 7 | * For details see the LICENSE file distributed with DubinsPathPlanner. 8 | */ 9 | // FIXME write these tests 10 | --------------------------------------------------------------------------------