├── .travis.yml ├── LICENSE ├── README.md ├── gdip ├── CMakeLists.txt ├── CMakeLists.txt.in ├── GDIP.md ├── clean.sh ├── core │ ├── CMakeLists.txt │ ├── src │ │ └── opendubins │ │ │ ├── angleinterval.cpp │ │ │ ├── angleinterval.h │ │ │ ├── arc.cpp │ │ │ ├── arc.h │ │ │ ├── circle.cpp │ │ │ ├── circle.h │ │ │ ├── dip.cpp │ │ │ ├── dmath.h │ │ │ ├── dtype.h │ │ │ ├── dubins.cpp │ │ │ ├── dubins.h │ │ │ ├── dubinsmaneuver.cpp │ │ │ ├── dubinspath.cpp │ │ │ ├── dubinspath.h │ │ │ ├── gdip.cpp │ │ │ ├── line.cpp │ │ │ ├── line.h │ │ │ ├── path.cpp │ │ │ ├── path.h │ │ │ ├── point.h │ │ │ ├── state.cpp │ │ │ ├── state.h │ │ │ ├── stateatdistance.h │ │ │ └── vector.h │ └── test │ │ ├── angleintervaltest.cpp │ │ ├── circletest.cpp │ │ ├── diptest.cpp │ │ ├── dubinstest.cpp │ │ ├── gdiptest.cpp │ │ └── speedtest.cpp ├── examples │ ├── CMakeLists.txt │ └── src │ │ ├── gdipexample.cpp │ │ └── hellodubins.cpp ├── install.sh └── wrappers │ ├── CMakeLists.txt │ └── src │ ├── gdipJuliaAPI.cpp │ ├── gdipJuliaAPI.h │ ├── gdipPythonAPI.cpp │ └── gdipPythonAPI.h ├── julia ├── GDIP.jl ├── basic_gdip_example.jl ├── create_gif_example.sh ├── problems │ └── gdip-n10.txt └── rss18_dtrp.jl └── python ├── basic_gdip_example.py ├── create_gif_example.sh ├── dubinswrapper.py ├── problems └── gdip-n10.txt └── rss18_dtrp.py /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | install: make get-deps 4 | 5 | matrix: 6 | include: 7 | - os: linux 8 | compiler: gcc 9 | - os: linux 10 | compiler: clang 11 | - os: osx 12 | - os: windows 13 | 14 | install: 15 | - cd gdip 16 | - ./install.sh 17 | - cd - 18 | 19 | script: 20 | - gdip/bin/hellodubins 21 | - gdip/bin/dubinstest 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2018, comrob 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Generalized Dubins Interval Problem (GDIP) 2 | 3 | [![Build Status](https://app.travis-ci.com/comrob/gdip.svg?branch=master)](https://app.travis-ci.com/github/comrob/gdip) 4 | 5 | This repository provides the optimal solution of the GDIP wchich enables to find a tight lower-bound for the Dubins Traveling Salesman Problem with Neighborhoods (DTSPN). The provided source codes are implemented in C++11 and presented at RSS 2018 conference. Please, cite the journal version. 6 | 7 | ``` 8 | @article{vana20auro, 9 | author = {Váňa, Petr and Faigl, Jan}, 10 | title = {Optimal Solution of the Generalized Dubins Interval Problem Finding the Shortest Curvature-constrained Path Through a Set of Regions}, 11 | journal = {Autonomous Robots}, 12 | volume = {44}, 13 | number = {7}, 14 | pages = {1359--1376}, 15 | year = {2020}, 16 | doi = {10.1007/s10514-020-09932-x} 17 | } 18 | ``` 19 | 20 | This paper has been nominated for **the best student paper** at RSS 2018 conference. 21 | 22 | ## GDIP example 23 | 24 | ![GDIP example](https://raw.githubusercontent.com/petvana/images/master/gdip/basic-gdip-example-small.gif) 25 | 26 | ## DTRP solution 27 | 28 | Feasible solution (blue) and the corresponding lower bound (red). 29 | 30 | ![DTRP solution](https://raw.githubusercontent.com/petvana/images/master/gdip/rss-example-small.gif) 31 | 32 | ## Basic usage in C++ and library compilation 33 | 34 | [C++ library](gdip/GDIP.md) 35 | -------------------------------------------------------------------------------- /gdip/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(opendubins) 3 | 4 | # Debug / Release 5 | if(CMAKE_BUILD_TYPE MATCHES Debug) 6 | MESSAGE("Compiling in the Debug mode") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O0") 8 | else() 9 | MESSAGE("Compiling in the Release mode") 10 | if (WIN32) 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2") 12 | else() 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") # -march=native 14 | endif() 15 | set(CMAKE_BUILD_TYPE Release) 16 | endif() 17 | 18 | if (WIN32) 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -W4") 20 | else() 21 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") 22 | endif() 23 | 24 | # For debug and profilling: 25 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pg") 26 | 27 | MESSAGE("CMAKE_CXX_COMPILER_ID ${CMAKE_CXX_COMPILER_ID}") 28 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 29 | MESSAGE("Disable error messsages for unused-command-line-argument (Clang only)") 30 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error=unused-command-line-argument -Qunused-arguments") 31 | endif() 32 | MESSAGE("CMAKE_C_COMPILER_ID ${CMAKE_C_COMPILER_ID}") 33 | if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang") 34 | MESSAGE("Disable error messsages for unused-command-line-argument (Clang only)") 35 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-error=unused-command-line-argument -Qunused-arguments") 36 | endif() 37 | 38 | find_program(CCACHE_FOUND ccache) 39 | if(CCACHE_FOUND) 40 | set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) 41 | set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) 42 | endif(CCACHE_FOUND) 43 | 44 | # Download and unpack googletest at configure time 45 | configure_file(CMakeLists.txt.in googletest-download/CMakeLists.txt) 46 | execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 47 | RESULT_VARIABLE result 48 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download ) 49 | if(result) 50 | message(FATAL_ERROR "CMake step for googletest failed: ${result}") 51 | endif() 52 | execute_process(COMMAND ${CMAKE_COMMAND} --build . 53 | RESULT_VARIABLE result 54 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download ) 55 | if(result) 56 | message(FATAL_ERROR "Build step for googletest failed: ${result}") 57 | endif() 58 | 59 | # Prevent overriding the parent project's compiler/linker 60 | # settings on Windows 61 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 62 | 63 | # Add googletest directly to our build. This defines 64 | # the gtest and gtest_main targets. 65 | add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src 66 | ${CMAKE_CURRENT_BINARY_DIR}/googletest-build 67 | EXCLUDE_FROM_ALL) 68 | 69 | set(GTEST_INCLUDE_DIRS "${gtest_SOURCE_DIR}/include") 70 | message("GTEST_INCLUDE_DIRS = ${GTEST_INCLUDE_DIRS}") 71 | 72 | set(GTEST_BOTH_LIBRARIES "gtest_main") 73 | message("GTEST_BOTH_LIBRARIES = ${GTEST_BOTH_LIBRARIES}") 74 | 75 | # The gtest/gtest_main targets carry header search path 76 | # dependencies automatically when using CMake 2.8.11 or 77 | # later. Otherwise we have to add them here ourselves. 78 | if (CMAKE_VERSION VERSION_LESS 2.8.11) 79 | include_directories("${GTEST_INCLUDE_DIRS}") 80 | endif() 81 | 82 | set(GTEST_FOUND true) 83 | 84 | if(GTEST_FOUND) 85 | MESSAGE("GTEST OK") 86 | else() 87 | MESSAGE("GTEST not found") 88 | endif() 89 | 90 | # DIRECTORIES 91 | add_subdirectory(core) 92 | add_subdirectory(examples) 93 | add_subdirectory(wrappers) 94 | 95 | -------------------------------------------------------------------------------- /gdip/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | 3 | project(googletest-download NONE) 4 | 5 | include(ExternalProject) 6 | ExternalProject_Add(googletest 7 | GIT_REPOSITORY https://github.com/google/googletest.git 8 | GIT_TAG release-1.12.1 9 | SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" 10 | BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" 11 | CONFIGURE_COMMAND "" 12 | BUILD_COMMAND "" 13 | INSTALL_COMMAND "" 14 | TEST_COMMAND "" 15 | ) -------------------------------------------------------------------------------- /gdip/GDIP.md: -------------------------------------------------------------------------------- 1 | # Core of GDIP library (C++) 2 | 3 | The code can be compiled by the prepared script which invokes CMake compilation: 4 | 5 | ```bash 6 | ./install.sh 7 | ``` 8 | 9 | ## Dependencies 10 | 11 | ### Mandatory 12 | 13 | make 14 | 15 | cmake 16 | 17 | C++ compiler (C++11 compatible) 18 | 19 | ### Optional 20 | 21 | gtest - for generating the provided tests 22 | 23 | ccache - for catching compiled results (speeding up re-copilation) 24 | 25 | ## Basic usage in C++ 26 | 27 | Local compilation is possible by running prepared install_local.sh script. It creates these directories: include, lib, bin, build. 28 | 29 | The following example shows how to utilize this library for finding the optimal solution of the GDIP. This example is provided in the file gdipexample.cpp. 30 | 31 | ```c++ 32 | #include 33 | 34 | #include "opendubins/dubins.h" 35 | 36 | using namespace std; 37 | using namespace opendubins; 38 | 39 | int main() { 40 | 41 | cout << "Hello, this is example of optimal solution of the GDIP!" << endl; 42 | 43 | // Centers of two given regions 44 | Point p1(0,0); 45 | Point p2(5,5); 46 | 47 | // Radii of the regions 48 | double region1radius = 0.5; 49 | double region2radius = 2; 50 | 51 | cout << "Let have two locations: " << endl << p1 << "," << endl << p2 << "." << endl; 52 | 53 | cout << "Now, we define intervals of the corresponding heading angles." << endl; 54 | /* AngleInterval has three parameters: 55 | * 1) Position of the center 56 | * 2) Right limit of the angle interval [ 2) = minimum angle ] 57 | * 3) Interval size [ 2) + 3) = maximum angle ] 58 | */ 59 | AngleInterval a1(p1, 0, M_PI/2); 60 | AngleInterval a2(p2, M_PI, M_PI/2); 61 | /* Thus, interval of a1 is [0, M_PI/2] 62 | * and nterval of a2 is [M_PI, 3*M_PI/2]. 63 | */ 64 | 65 | cout << "The angle intervals are: " << endl << a1 << "," << endl << a2 << "." << endl; 66 | cout << "The goal is to find the shortest Dubins path between two regions." 67 | << "The first region has radius " << region1radius << ", and the second one " << region2radius << "." << endl; 68 | 69 | // Minimum turning radius 70 | double radius = 1.0; 71 | 72 | Dubins d = Dubins(a1, a2, radius, region1radius, region2radius); 73 | cout << "For radius=" << radius << " the shortest path (optimal GDIP solution) is:" << endl << d << endl; 74 | 75 | // Check if the endpoints are in the given disk-shaped regions. 76 | cout << "Chech the first endpoint (" << p1.distance(d.start.point) << " <= " << region1radius << ")." << endl; 77 | cout << "Chech the second endpoint (" << p2.distance(d.end.point) << " <= " << region2radius << ")." << endl; 78 | 79 | return 0; 80 | } 81 | ``` 82 | -------------------------------------------------------------------------------- /gdip/clean.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | rm -rf include bin lib build 4 | -------------------------------------------------------------------------------- /gdip/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(opendubins_core) 3 | 4 | include_directories("src") 5 | 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 7 | 8 | set(SOURCE_FILES ${DUBINS_SRC} 9 | src/opendubins/arc.cpp 10 | src/opendubins/circle.cpp 11 | src/opendubins/dubins.cpp 12 | src/opendubins/dubinsmaneuver.cpp 13 | src/opendubins/dip.cpp 14 | src/opendubins/gdip.cpp 15 | src/opendubins/line.cpp 16 | src/opendubins/path.cpp 17 | src/opendubins/state.cpp 18 | src/opendubins/dubinspath.cpp 19 | src/opendubins/angleinterval.cpp 20 | 21 | src/opendubins/stateatdistance.h 22 | src/opendubins/vector.h 23 | src/opendubins/point.h 24 | src/opendubins/dmath.h 25 | src/opendubins/dtype.h) 26 | 27 | add_library(opendubins_core STATIC ${SOURCE_FILES}) 28 | 29 | if (GTEST_FOUND) 30 | 31 | include_directories("${GTEST_INCLUDE_DIRS}") 32 | 33 | add_executable(dubinstest 34 | test/dubinstest.cpp 35 | test/diptest.cpp 36 | test/gdiptest.cpp 37 | test/circletest.cpp 38 | test/angleintervaltest.cpp 39 | test/speedtest.cpp 40 | ) 41 | 42 | target_link_libraries(dubinstest opendubins_core) 43 | target_link_libraries(dubinstest ${GTEST_BOTH_LIBRARIES} ) 44 | 45 | add_test(core_tests dubinstest) 46 | 47 | install(TARGETS dubinstest DESTINATION bin) 48 | 49 | endif () 50 | 51 | install(DIRECTORY src/ DESTINATION include FILES_MATCHING PATTERN "*.h") 52 | install(TARGETS opendubins_core DESTINATION lib) 53 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/angleinterval.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file angleinterval.cpp 3 | * @author Petr Vana 4 | * @brief Interval of heading angles utilized in both DIP and GDIP formulation 5 | */ 6 | 7 | #include "angleinterval.h" 8 | 9 | using namespace opendubins; 10 | using namespace std; 11 | 12 | namespace opendubins { 13 | 14 | vector AngleInterval::mergeIntervals(vector input){ 15 | decltype(input) merged; 16 | decltype(input) twice; 17 | decltype(input) filtered; 18 | 19 | if(input.size() > 0) { 20 | Point center = input[0].point; 21 | 22 | // sort intervals 23 | sort(input.begin(), input.end()); 24 | 25 | // make second copy of intervals from [0, 2PI] to [2PI, 4PI] 26 | twice = input; 27 | for (auto i : input) { 28 | i.rightDir += 2 * M_PI; 29 | twice.push_back(i); 30 | } 31 | 32 | // merge intervals in interval [0, 4PI] 33 | // last interval can end behind 4PI (but no problem here) 34 | double from = twice[0].rightDir; 35 | double to = from + twice[0].diff; 36 | for (unsigned int i = 1; i < twice.size(); ++i) { 37 | const auto &it = twice[i]; 38 | if (it.rightDir <= to) { 39 | to = max(to, (it.rightDir + it.diff)); 40 | } else { 41 | // cannot use constructor directly - a normalization is included 42 | AngleInterval ai; 43 | ai.rightDir = from; 44 | ai.diff = to - from; 45 | merged.push_back(ai); 46 | from = it.rightDir; 47 | to = it.rightDir + it.diff; 48 | } 49 | } 50 | // cannot use constructor directly - a normalization is included 51 | AngleInterval ai; 52 | ai.rightDir = from; 53 | ai.diff = to - from; 54 | merged.push_back(ai); 55 | 56 | // filter interval and reduce them to interval [2PI, 4PI] 57 | for(auto i : merged){ 58 | const double M = 2*M_PI; 59 | auto a = i.rightDir - M; 60 | auto b = a + i.diff; 61 | if(a < M && b > 0){ 62 | a = max(a, 0.0); 63 | b = min(b, M); 64 | filtered.push_back(AngleInterval(center, a, b-a)); 65 | } 66 | } 67 | } 68 | 69 | return filtered; 70 | } 71 | 72 | ostream& operator<<(ostream &os, const AngleInterval &d) { 73 | os << "AngleInterval: (" << d.rightDir << "; " << (d.rightDir + d.diff) << "), point " << d.point; 74 | return os; 75 | } 76 | 77 | } -------------------------------------------------------------------------------- /gdip/core/src/opendubins/angleinterval.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file angleinterval.h 3 | * @author Petr Vana 4 | * @brief Interval of heading angles utilized in both DIP and GDIP formulation 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include "dmath.h" 12 | #include "state.h" 13 | 14 | namespace opendubins { 15 | 16 | struct AngleInterval { 17 | 18 | // point to which the angle interval belongs 19 | Point point; 20 | 21 | // right angle from the interval 22 | // startAngle \in <0,2*PI) 23 | double rightDir; 24 | 25 | // diff between left and right angle <0,2*PI> 26 | // diff = 0 >> single direction 27 | // diff = 2*PI >> every possible dirrection 28 | double diff; 29 | 30 | int resolution; 31 | 32 | AngleInterval() : point(Point(GDIP_NAN, GDIP_NAN)), rightDir(GDIP_NAN), diff(GDIP_NAN) { 33 | resolution = -1; 34 | }; 35 | 36 | AngleInterval(Point point, double rightDir, double diff) : point(point), rightDir(rightDir) { 37 | this->diff = angleToLeft(0, diff); 38 | resolution = -1; 39 | normalize(); 40 | }; 41 | 42 | AngleInterval(State state, double diff) : point(state.point), rightDir(state.ang) { 43 | this->diff = angleToLeft(0, diff); 44 | resolution = -1; 45 | normalize(); 46 | }; 47 | 48 | AngleInterval(State state) : point(state.point), rightDir(state.ang), diff(2 * M_PI) { 49 | resolution = 1; 50 | normalize(); 51 | }; 52 | 53 | 54 | AngleInterval(Point point) : point(point), rightDir(0), diff(2 * M_PI) { 55 | resolution = 1; 56 | normalize(); 57 | }; 58 | 59 | AngleInterval(Point point, double rightDir, double diff, int resolution) : point(point), rightDir(rightDir), 60 | diff(diff), 61 | resolution(resolution) { 62 | normalize(); 63 | }; 64 | 65 | inline double getRight() const { 66 | return rightDir; 67 | } 68 | 69 | inline double getLeft() const { 70 | return rightDir + diff; 71 | } 72 | 73 | inline State getLeftState() const { 74 | return State(point, getLeft()); 75 | } 76 | 77 | inline State getRightState() const { 78 | return State(point, getRight()); 79 | } 80 | 81 | inline Vector getLeftDir() const { 82 | return Vector(getLeft()); 83 | } 84 | 85 | inline Vector getRightDir() const { 86 | return Vector(getRight()); 87 | } 88 | 89 | inline double inInterval(double direction) const { 90 | return angleToLeft(rightDir, direction) <= diff; 91 | } 92 | 93 | inline double inIntervalWithTollerance(double direction, double tollerance) { 94 | if (angleToLeft(rightDir, direction) <= diff + tollerance) { 95 | return true; 96 | } else { 97 | if (angleToLeft(rightDir, direction) + tollerance > 2 * M_PI) { 98 | return true; 99 | } else { 100 | return false; 101 | } 102 | } 103 | } 104 | 105 | inline AngleInterval splitInterval() { 106 | diff /= 2; 107 | resolution *= 2; 108 | AngleInterval ret(point, getLeft(), diff); 109 | ret.resolution = resolution; 110 | return ret; 111 | } 112 | 113 | inline AngleInterval reverse() const { 114 | double newAngle = angleToLeft(0, rightDir + M_PI); 115 | return AngleInterval(point, newAngle, diff, resolution); 116 | } 117 | 118 | inline bool operator<(const AngleInterval &o) const { 119 | return rightDir < o.rightDir; 120 | } 121 | 122 | inline bool operator==(const AngleInterval &o) const { 123 | return (point == o.point) && (rightDir == o.rightDir) && (diff == o.diff); 124 | } 125 | 126 | // hash point and angles 127 | inline size_t hash() const { 128 | auto h1 = point.hash(); 129 | auto h2 = std::hash()(diff); 130 | auto h3 = std::hash()(rightDir); 131 | return h1 ^ (h2 << 1) ^ (h3 << 2); 132 | } 133 | 134 | // hash only angles 135 | inline size_t angle_hash() const { 136 | auto h1 = std::hash()(diff); 137 | auto h2 = std::hash()(rightDir); 138 | return h1 ^ (h2 << 1); 139 | } 140 | 141 | inline bool isValid() const { 142 | return !std::isnan(rightDir); 143 | } 144 | 145 | inline void normalize() { 146 | rightDir = angleToLeft(0, rightDir); 147 | } 148 | 149 | static std::vector mergeIntervals(std::vector input); 150 | }; 151 | 152 | std::ostream &operator<<(std::ostream &os, const AngleInterval &d); 153 | } 154 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/arc.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file arc.cpp 3 | * @author Petr Vana 4 | * @brief Arc is a basic segment in Dubins maneuver 5 | */ 6 | 7 | #include "arc.h" 8 | 9 | namespace opendubins { 10 | 11 | State Arc::getState(double len) const { 12 | return getStateByAngle(len * sgn(angle) / radius); 13 | } 14 | 15 | StateAtDistance Arc::getClosestStateAndDistance(const Point &p) const { 16 | StateAtDistance ret; 17 | 18 | // start state 19 | ret.state = state; 20 | ret.distance = 0; 21 | 22 | // end state 23 | auto end = getEnd(); 24 | if (end.point.distance(p) < ret.state.point.distance(p)) { 25 | ret.state = end; 26 | ret.distance = getLength(); 27 | } 28 | 29 | // center state 30 | if (angle > 0) { // left turn 31 | auto dir = (p - getCenter()).left().getAngle(); 32 | auto turn = angleToLeft(state.ang, dir); 33 | auto turnLen = turn * radius; 34 | if (turnLen < getLength()) { 35 | auto actPos = getState(turnLen); 36 | if (actPos.point.distance(p) < ret.state.point.distance(p)) { 37 | ret.state = actPos; 38 | ret.distance = turnLen; 39 | } 40 | } 41 | } else { // right turn 42 | auto dir = (p - getCenter()).right().getAngle(); 43 | auto turn = angleToRight(state.ang, dir); 44 | auto turnLen = -turn * radius; 45 | if (turnLen < getLength()) { 46 | auto actPos = getState(turnLen); 47 | if (actPos.point.distance(p) < ret.state.point.distance(p)) { 48 | ret.state = actPos; 49 | ret.distance = turnLen; 50 | } 51 | } 52 | } 53 | 54 | return ret; 55 | } 56 | 57 | StateAtDistance Arc::intersectionPoint(const Line &line) const { 58 | double rad = getLength() + line.getLength(); 59 | if ((state.point - line.p1).lengthSquared() > rad * rad) { 60 | return StateAtDistance(State(), 0); 61 | } 62 | 63 | // calculate two points of intersection 64 | Vector dir = line.getDirection().normalize(); 65 | Vector normal = dir.left(); 66 | double distance = normal.dotProduct(getCenter() - line.p1); 67 | 68 | // vector to closest point on line from center of arc 69 | Vector vDistance = -distance * normal; 70 | 71 | if (distance > radius) { 72 | return StateAtDistance(State(), 0); 73 | } 74 | 75 | double tangentAngle = vDistance.getAngle() + (angle > 0 ? M_PI / 2 : -M_PI / 2); 76 | double diffAngle = acos(fabs(distance) / radius); 77 | 78 | double ang1 = tangentAngle + diffAngle; 79 | double ang2 = tangentAngle - diffAngle; 80 | 81 | if (angle > 0) { // left 82 | double turn1 = angleToLeft(state.ang, ang1); 83 | double turn2 = angleToLeft(state.ang, ang2); 84 | 85 | double less = std::min(turn1, turn2); 86 | double more = std::max(turn1, turn2); 87 | 88 | if (less <= angle) { 89 | State p = getStateByAngle(less); 90 | double dist = dir.dotProduct(p.point - line.p1); 91 | 92 | if (dist >= 0 && dist < line.getDirection().length()) { 93 | return StateAtDistance(p, less * radius); 94 | } 95 | 96 | if (more <= angle) { 97 | p = getStateByAngle(more); 98 | dist = dir.dotProduct(p.point - line.p1); 99 | 100 | if (dist >= 0 && dist < line.getDirection().length()) { 101 | return StateAtDistance(p, more * radius); 102 | } 103 | } 104 | } 105 | } else { 106 | double turn1 = angleToRight(state.ang, ang1); 107 | double turn2 = angleToRight(state.ang, ang2); 108 | 109 | double less = std::max(turn1, turn2); 110 | double more = std::min(turn1, turn2); 111 | 112 | if (less >= angle) { 113 | State p = getStateByAngle(less); 114 | double dist = dir.dotProduct(p.point - line.p1); 115 | 116 | if (dist >= 0 && dist < line.getDirection().length()) { 117 | return StateAtDistance(p, less * -radius); 118 | } 119 | 120 | if (more >= angle) { 121 | p = getStateByAngle(more); 122 | dist = dir.dotProduct(p.point - line.p1); 123 | 124 | if (dist >= 0 && dist < line.getDirection().length()) { 125 | return StateAtDistance(p, more * -radius); 126 | } 127 | } 128 | } 129 | } 130 | 131 | // todo - try to refactor 132 | 133 | return StateAtDistance(State(), 0); 134 | } 135 | 136 | Point Arc::getCenter() const { 137 | Point center = state.point; 138 | 139 | Vector toCenter = state.getNormalizedDirection().left(); 140 | toCenter *= radius; 141 | if (angle < 0) { 142 | toCenter *= -1; 143 | } 144 | 145 | center += toCenter; 146 | 147 | return center; 148 | } 149 | 150 | State Arc::getStateByAngle(double arcAngle) const { 151 | Point center = getCenter(); 152 | 153 | Vector dir = state.getNormalizedDirection() * radius; 154 | Vector norm = state.point - center; 155 | 156 | double aa = std::fabs(arcAngle); 157 | 158 | center += dir * std::sin(aa); 159 | center += norm * std::cos(aa); 160 | 161 | return State(center, state.ang + arcAngle); 162 | } 163 | 164 | State Arc::getEnd() const { 165 | return getStateByAngle(angle); 166 | } 167 | 168 | 169 | } 170 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/arc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file arc.h 3 | * @author Petr Vana 4 | * @brief Arc is a basic segment in Dubins maneuver 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "path.h" 10 | #include "line.h" 11 | #include "stateatdistance.h" 12 | 13 | namespace opendubins { 14 | 15 | class Arc : public Path { 16 | public: 17 | 18 | State state; 19 | double angle; 20 | double radius; 21 | 22 | Arc() : 23 | angle(GDIP_NAN), radius(GDIP_NAN) { 24 | } 25 | 26 | Arc(const State &state, double angle, double radius) : 27 | state(state), angle(angle), radius(radius) { 28 | } 29 | 30 | virtual State getState(double len) const; 31 | 32 | virtual StateAtDistance getClosestStateAndDistance(const Point &p) const; 33 | 34 | StateAtDistance intersectionPoint(const Line &line) const; 35 | 36 | Point getCenter() const; 37 | 38 | State getStateByAngle(double angle) const; 39 | 40 | State getEnd() const; 41 | 42 | inline double getLength() const { 43 | return std::fabs(getAngle()) * getRadius(); 44 | } 45 | 46 | inline double getAngle() const { return angle; } 47 | 48 | inline double getRadius() const { return radius; } 49 | 50 | inline State getStart() const { return state; } 51 | }; 52 | 53 | inline std::ostream &operator<<(std::ostream &os, const Arc &a) { 54 | os << "Arc: from={" << a.state << "} angle=" << a.angle << " RADIUS=" << a.radius; 55 | return os; 56 | } 57 | 58 | } 59 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/circle.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file circle.cpp 3 | * @author Petr Vana 4 | * @brief Circle class used for calculating intersections, etc. 5 | */ 6 | 7 | #include "circle.h" 8 | 9 | using namespace std; 10 | 11 | namespace opendubins { 12 | 13 | Circle::Circle(const Point ¢er, const double &radius) { 14 | this->center = center; 15 | this->radius = radius; 16 | } 17 | 18 | Circle::~Circle() { } 19 | 20 | bool Circle::pointInCircle(const Point& p) const { 21 | return center.distanceSquared(p) <= radius * radius; 22 | } 23 | 24 | Point Circle::getCenter() const { 25 | return center; 26 | } 27 | 28 | double Circle::perimeterLength() const { 29 | return 2 * M_PI * radius; 30 | } 31 | 32 | 33 | tuple Circle::halfLineIntersections(const State &pos) const { 34 | int count = 0; 35 | Point ps[2]; 36 | 37 | // calculate two points of intersection 38 | Vector dir = pos.getNormalizedDirection(); // (already normalized) 39 | Vector normal = dir.left(); 40 | double distance = normal.dotProduct(center - pos.point); 41 | 42 | // vector to closest point on line from center of arc 43 | Vector vDistance = -distance * normal; 44 | 45 | if (distance < radius) { 46 | double tangentAngle = vDistance.getAngle(); 47 | double diffAngle = acos(fabs(distance) / radius); 48 | 49 | double ang1 = tangentAngle + diffAngle; 50 | double ang2 = tangentAngle - diffAngle; 51 | 52 | auto p1 = center + Vector(ang1) * radius; 53 | auto p2 = center + Vector(ang2) * radius; 54 | 55 | if (dir.dotProduct(p1 - pos.point) > 0) { 56 | ps[count++] = p1; 57 | } 58 | 59 | if (dir.dotProduct(p2 - pos.point) > 0) { 60 | ps[count++] = p2; 61 | } 62 | } 63 | 64 | return tuple(count, ps[0], ps[1]); 65 | } 66 | 67 | Point Circle::halfLineIntersection(const State &pos) const { 68 | tuple ints = halfLineIntersections(pos); 69 | int count = get<0>(ints); 70 | 71 | if (count > 1) { 72 | auto len1 = pos.point.distance(get<1>(ints)); 73 | if (count == 1) { 74 | return get<1>(ints); 75 | } else { 76 | auto len2 = pos.point.distance(get<2>(ints)); 77 | if (len1 < len2) { 78 | return get<1>(ints); 79 | } else { 80 | return get<2>(ints); 81 | } 82 | } 83 | } 84 | return Point::getInvalid(); 85 | } 86 | 87 | tuple Circle::firstIntersection(const Arc& arc) const{ 88 | auto arcCenter = arc.getCenter(); 89 | Vector diff = center - arcCenter; 90 | 91 | double diffLen = diff.length(); 92 | 93 | // alpha = acos((b*b + c*c - a*a)/(2*b*c)); 94 | double a = radius; 95 | double b = arc.radius; 96 | double c = diffLen; 97 | 98 | double arccos = (b*b + c*c - a*a)/(2*b*c); 99 | if(arccos >= -1 && arccos <= 1){ 100 | double alpha = acos(arccos); 101 | 102 | double orient = arc.getAngle() > 0 ? M_PI_2 : -M_PI_2; 103 | 104 | double dir1 = diff.getAngle() + alpha + orient; 105 | double dir2 = diff.getAngle() - alpha + orient; 106 | 107 | double stAng = arc.getStart().ang; 108 | 109 | double turn1 = arc.getAngle() > 0 ? angleToLeft(stAng, dir1) 110 | : angleToRight(stAng, dir1); 111 | double turn2 = arc.getAngle() > 0 ? angleToLeft(stAng, dir2) 112 | : angleToRight(stAng, dir2); 113 | 114 | if(fabs(turn1) < fabs(turn2)){ 115 | return tuple(arc.getState(fabs(turn1 * arc.radius)), fabs(turn1 * arc.radius)); 116 | }else{ 117 | return tuple(arc.getState(fabs(turn2 * arc.radius)), fabs(turn2 * arc.radius)); 118 | } 119 | 120 | } 121 | 122 | return tuple(State::getInvalid(), -1); 123 | } 124 | 125 | AngleInterval Circle::getIntersectionAngleInterval(const Circle& other) const { 126 | AngleInterval interval = AngleInterval(); 127 | 128 | Vector diff = other.center - center; 129 | double diffLen = diff.length(); 130 | 131 | // alpha = acos((b*b + c*c - a*a)/(2*b*c)); 132 | double a = other.radius; 133 | double b = radius; 134 | double c = diffLen; 135 | 136 | double arccos = (b*b + c*c - a*a)/(2*b*c); 137 | if(arccos >= -1 && arccos <= 1){ 138 | double alpha = acos(arccos); 139 | double diffAngle = diff.getAngle(); 140 | return AngleInterval(center, diffAngle - alpha, 2*alpha); 141 | } 142 | 143 | // this circle is completely in the other one 144 | if(diffLen < other.radius && radius < other.radius){ 145 | return AngleInterval(center, 0, 2*M_PI); 146 | } 147 | 148 | return interval; 149 | } 150 | 151 | vector Circle::getIntersectionAngleIntervals(const vector circles) const { 152 | vector intersections; 153 | 154 | for(auto & c : circles){ 155 | auto inter = getIntersectionAngleInterval(c); 156 | if(inter.isValid()){ 157 | intersections.push_back(inter); 158 | } 159 | } 160 | 161 | return AngleInterval::mergeIntervals(intersections); 162 | } 163 | 164 | Vector Circle::shortestVectorInSector(const AngleInterval& sector) const { 165 | // zero distance 166 | if(pointInCircle(sector.point)){ 167 | return Vector(0,0); 168 | } 169 | 170 | // directly to the circle 171 | auto diff = center - sector.point; 172 | if(sector.inInterval(diff.getAngle())){ 173 | auto diff2 = diff.normalize() * -radius; 174 | return (center + diff2) - sector.point; 175 | } 176 | 177 | // at the edge of the sector 178 | Point p1 = halfLineIntersection(sector.getLeftState()); 179 | Point p2 = halfLineIntersection(sector.getRightState()); 180 | 181 | Point ret = Point::getInvalid(); 182 | 183 | double len = numeric_limits::max(); 184 | if(p1.isValid()){ 185 | double nLen = p1.distance(sector.point); 186 | if(nLen < len){ 187 | len = nLen; 188 | ret = p1; 189 | } 190 | } 191 | if(p2.isValid()){ 192 | double nLen = p2.distance(sector.point); 193 | if(nLen < len){ 194 | //len = nLen; // not used anymore 195 | ret = p2; 196 | } 197 | } 198 | 199 | if(ret.isValid()){ 200 | return ret - sector.point; 201 | }else{ 202 | return Vector::getInvalid(); 203 | } 204 | } 205 | 206 | Point Circle::closestPointInSector(const AngleInterval& sector) const { 207 | return sector.point + shortestVectorInSector(sector); 208 | } 209 | 210 | } 211 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/circle.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file circle.h 3 | * @author Petr Vana 4 | * @brief Circle class used for calculating intersections, etc. 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include "state.h" 12 | #include "arc.h" 13 | #include "angleinterval.h" 14 | 15 | namespace opendubins { 16 | 17 | struct Circle { 18 | 19 | Point center; 20 | double radius; 21 | 22 | inline Circle() : center(Point(GDIP_NAN, GDIP_NAN)), radius(GDIP_NAN) { }; 23 | 24 | Circle(const Point ¢er, const double &radius); 25 | 26 | virtual ~Circle(); 27 | 28 | bool pointInCircle(const Point&) const; 29 | 30 | Point getCenter() const; 31 | 32 | double perimeterLength() const; 33 | 34 | std::tuple halfLineIntersections(const State& pos) const; 35 | Point halfLineIntersection(const State& pos) const; 36 | 37 | inline double getRadius() { return radius; } 38 | 39 | std::tuple firstIntersection(const Arc& arc) const; 40 | 41 | // find circle intersection interval 42 | // return AngleInterval which stands for intersection arc of *this circle and starts in *this.center 43 | // if there is no intersection - invalid interval is retuned (see AngleInterval::isValid()) 44 | // if the first circle is completely in the other one [0,2PI] interval is returned 45 | AngleInterval getIntersectionAngleInterval(const Circle& other) const; 46 | 47 | // find circles intersection interval 48 | // intervals are merged - sum of AngleInterval 49 | std::vector getIntersectionAngleIntervals(const std::vector circles) const; 50 | 51 | // find the shortest vector from the sector.point 52 | // wanted vector need to intersect both the circle and the sector 53 | Vector shortestVectorInSector(const AngleInterval& sector) const; 54 | 55 | // find the closest point from the sector.point 56 | // wanted point need to intersect both the circle and the sector 57 | Point closestPointInSector(const AngleInterval& sector) const; 58 | }; 59 | 60 | } 61 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/dmath.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dmath.h 3 | * @author Petr Vana 4 | * @brief Collection of math function for the OpenDubins library 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace opendubins { 18 | 19 | #ifndef M_PI 20 | #define M_PI 3.141592653589793238462643383279502884 /* pi use l for long double */ 21 | #endif 22 | 23 | #ifndef M_PI_2 24 | #define M_PI_2 (M_PI/2) 25 | #endif 26 | 27 | #define GDIP_NAN std::numeric_limits::quiet_NaN() 28 | 29 | const double TOLERANCE = 1e-5; 30 | 31 | static unsigned _dmath_current_seed = 42; 32 | 33 | static std::default_random_engine opendubins_random_generator; 34 | 35 | inline double myRandom() { 36 | //return rand() / (myFloat)RAND_MAX; 37 | //return random() / (double) RAND_MAX; 38 | return std::uniform_real_distribution(0,1)(opendubins_random_generator); 39 | } 40 | 41 | inline void setSeed(unsigned seed) { 42 | /*if (seed == 42) { 43 | struct timespec ts; // C11 standard 44 | timespec_get(&ts, TIME_UTC); 45 | seed = (ts.tv_nsec); 46 | }*/ 47 | _dmath_current_seed = seed; 48 | opendubins_random_generator = std::default_random_engine(seed); 49 | } 50 | 51 | inline int myIntRandom(const int size) { 52 | //return rand() % size; 53 | //return random() % size; 54 | return std::uniform_int_distribution(0,size-1)(opendubins_random_generator); 55 | } 56 | 57 | template 58 | constexpr int sgn(T val) { 59 | return (T(0) < val) - (val < T(0)); 60 | } 61 | 62 | inline double angleToLeft(const double &ang1, const double &ang2) { 63 | double ret = ang2 - ang1; 64 | 65 | while (ret > 2 * M_PI) { 66 | ret -= 2 * M_PI; 67 | } 68 | while (ret < 0) { 69 | ret += 2 * M_PI; 70 | } 71 | return ret; 72 | } 73 | 74 | inline double angleToRight(const double &ang1, const double &ang2) { 75 | double ret = ang2 - ang1; 76 | 77 | while (ret < -2 * M_PI) { 78 | ret += 2 * M_PI; 79 | } 80 | while (ret > 0) { 81 | ret -= 2 * M_PI; 82 | } 83 | 84 | return ret; 85 | } 86 | 87 | inline double angleToSide(const double &ang1, const double &ang2, const bool& isLeft){ 88 | return isLeft ? angleToLeft(ang1, ang2) : angleToRight(ang1, ang2); 89 | } 90 | 91 | inline double checkToleranceLeft(double turn) { 92 | return (turn > 2 * M_PI - TOLERANCE && turn < 2 * M_PI + TOLERANCE) ? 0 : turn; 93 | } 94 | 95 | inline double checkToleranceRight(double turn) { 96 | return (turn < -2 * M_PI + TOLERANCE && turn > -2 * M_PI - TOLERANCE) ? 0 : turn; 97 | } 98 | 99 | inline void randomOrder(std::vector &order, int size) { 100 | order.clear(); 101 | for (int i = 0; i < size; i++) 102 | order.push_back(i); 103 | std::random_shuffle(order.begin(), order.end(), myIntRandom); 104 | } 105 | 106 | constexpr int modulo(int a, int b) { 107 | return (a + b) % b; 108 | } 109 | 110 | } 111 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/dtype.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dtype.h 3 | * @author Petr Vana 4 | * @brief Types of Dubins maneuver 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | namespace opendubins { 12 | 13 | enum struct DType { 14 | Unknown, 15 | 16 | RSR, 17 | LSL, 18 | RSL, 19 | LSR, 20 | RLR, 21 | LRL, 22 | 23 | // Dubins Interval Problem ---------------------------------------- 24 | // CSC and CCC maneuvers used from Dubins maneuver 25 | DIP_S, 26 | DIP_Rp, 27 | DIP_Lp, 28 | DIP_RS, 29 | DIP_LS, 30 | DIP_SR, 31 | DIP_SL, 32 | DIP_RLp, 33 | DIP_LRp, 34 | DIP_RpL, 35 | DIP_LpR, 36 | DIP_LSL, 37 | DIP_RSR, 38 | DIP_LSR, 39 | DIP_RSL, 40 | DIP_LRL, 41 | DIP_RLR, 42 | 43 | // special cases - this should never happen 44 | DIP_LpRp, 45 | DIP_RpLp, 46 | 47 | // Generalized Dubins Interval Problem ------------------------- 48 | GDIP_NO, 49 | GDIP_S, 50 | GDIP_R, 51 | GDIP_RS, 52 | GDIP_L, 53 | GDIP_LS, 54 | GDIP_SR, 55 | GDIP_SL, 56 | GDIP_RSR, 57 | GDIP_LSL, 58 | GDIP_RSL, 59 | GDIP_LSR, 60 | GDIP_LRL, 61 | GDIP_RLR, 62 | GDIP_Rp, 63 | GDIP_Lp, 64 | GDIP_LRp, 65 | GDIP_RLp, 66 | GDIP_LpR, 67 | GDIP_RpL, 68 | }; 69 | 70 | #define DTYPE_CASE(x) case DType::x: s << #x << "-" << (int)DType::x ; break; 71 | 72 | inline std::ostream& operator<< (std::ostream& s, DType type){ 73 | switch (type){ 74 | case DType::Unknown: s << "Unknown"; break; 75 | 76 | DTYPE_CASE(RSR) 77 | DTYPE_CASE(LSL) 78 | DTYPE_CASE(RSL) 79 | DTYPE_CASE(LSR) 80 | DTYPE_CASE(RLR) 81 | DTYPE_CASE(LRL) 82 | 83 | // DIP cases 84 | DTYPE_CASE(DIP_S) 85 | DTYPE_CASE(DIP_Rp) 86 | DTYPE_CASE(DIP_Lp) 87 | DTYPE_CASE(DIP_RS) 88 | DTYPE_CASE(DIP_LS) 89 | DTYPE_CASE(DIP_SR) 90 | DTYPE_CASE(DIP_SL) 91 | DTYPE_CASE(DIP_RLp) 92 | DTYPE_CASE(DIP_LRp) 93 | DTYPE_CASE(DIP_RpL) 94 | DTYPE_CASE(DIP_LpR) 95 | DTYPE_CASE(DIP_LSL) 96 | DTYPE_CASE(DIP_RSR) 97 | DTYPE_CASE(DIP_LSR) 98 | DTYPE_CASE(DIP_RSL) 99 | DTYPE_CASE(DIP_LRL) 100 | DTYPE_CASE(DIP_RLR) 101 | 102 | // GDIP cases 103 | DTYPE_CASE(GDIP_NO) 104 | DTYPE_CASE(GDIP_S) 105 | DTYPE_CASE(GDIP_R) 106 | DTYPE_CASE(GDIP_RS) 107 | DTYPE_CASE(GDIP_L) 108 | DTYPE_CASE(GDIP_LS) 109 | DTYPE_CASE(GDIP_SR) 110 | DTYPE_CASE(GDIP_SL) 111 | DTYPE_CASE(GDIP_RSR) 112 | DTYPE_CASE(GDIP_LSL) 113 | DTYPE_CASE(GDIP_RSL) 114 | DTYPE_CASE(GDIP_LSR) 115 | DTYPE_CASE(GDIP_LRL) 116 | DTYPE_CASE(GDIP_RLR) 117 | DTYPE_CASE(GDIP_Rp) 118 | DTYPE_CASE(GDIP_Lp) 119 | DTYPE_CASE(GDIP_LRp) 120 | DTYPE_CASE(GDIP_RLp) 121 | DTYPE_CASE(GDIP_LpR) 122 | DTYPE_CASE(GDIP_RpL) 123 | 124 | default: s << "Invalid("<<(int)type<<")"; break; 125 | } 126 | return s; 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/dubins.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dubins.cpp 3 | * @author Petr Vana 4 | * @brief Implementation of the basic Dubins maneuver between two oriented end points 5 | */ 6 | 7 | #include "dubins.h" 8 | 9 | #include 10 | 11 | using namespace std; 12 | 13 | namespace opendubins { 14 | 15 | Dubins::Dubins() : len1(GDIP_NAN), len2(GDIP_NAN), len3(GDIP_NAN), type(DType::Unknown), radius(GDIP_NAN), 16 | isCCC(false), length(numeric_limits::max()) { 17 | } 18 | 19 | Dubins::Dubins(const Line &line) : Dubins(line.getStart(), false, 0, line.getLength(), 0, 1) { 20 | type = DType::Unknown; 21 | } 22 | 23 | Dubins::Dubins(State pos, bool isCCC, double le1, double le2, double le3, double rad) { 24 | start = pos; 25 | radius = rad; 26 | this->radius = rad; 27 | this->isCCC = isCCC; 28 | len1 = le1; 29 | len2 = le2; 30 | len3 = le3; 31 | type = DType::Unknown; 32 | calculateLength(); 33 | end = getSecondArc().getEnd(); 34 | } 35 | 36 | Dubins::~Dubins() { } 37 | 38 | bool Dubins::check() { 39 | return end.point.distance(getSecondArc().getEnd().point) < ((5 + 2 * length) * TOLERANCE); 40 | } 41 | 42 | State Dubins::getState(double len) const { 43 | Arc path1 = getFirstArc(); 44 | double l1 = path1.getLength(); 45 | if (len < l1) { 46 | return path1.getState(len); 47 | } 48 | 49 | double l2; 50 | if (isCCC) { 51 | Arc ca = getCenterArc(); 52 | l2 = ca.getLength(); 53 | if (len < l1 + l2) { 54 | return ca.getState(len - l1); 55 | } 56 | } else { 57 | Line cl = getCenter(); 58 | l2 = cl.getLength(); 59 | if (len < l1 + l2) { 60 | return cl.getState(len - l1); 61 | } 62 | } 63 | 64 | Arc path3 = getSecondArc(); 65 | return path3.getState(len - l1 - l2); 66 | } 67 | 68 | StateAtDistance Dubins::getClosestStateAndDistance(const Point &p) const { 69 | StateAtDistance closest; 70 | 71 | // initialize by the start position 72 | closest.state = start; 73 | closest.distance = 0; 74 | 75 | auto firstArc = getFirstArc(); 76 | auto secondArc = getSecondArc(); 77 | 78 | Line centerLine; 79 | Arc centerArc; 80 | 81 | Path* segments[3]; 82 | segments[0] = (Path*) & firstArc; 83 | if(isCCC){ 84 | centerArc = getCenterArc(); 85 | segments[1] = (Path*) & centerArc; 86 | }else{ 87 | centerLine = getCenter(); 88 | segments[1] = (Path*) & centerLine; 89 | } 90 | segments[2] = (Path*) & secondArc; 91 | 92 | // add distance from the start (sum length of previous segments) 93 | double distance = 0; 94 | // iterate over all 3 segments 95 | for(int i = 0; i < 3; i++){ 96 | Path* seg = segments[i]; 97 | auto testClosest = seg->getClosestStateAndDistance(p); 98 | if (testClosest.state.point.distance(p) < closest.state.point.distance(p)) { 99 | closest = testClosest; 100 | closest.distance += distance; 101 | } 102 | distance += seg->getLength(); 103 | } 104 | 105 | return closest; 106 | } 107 | 108 | Arc Dubins::getFirstArc() const { 109 | auto st = start; 110 | return Arc(st, len1, radius); 111 | } 112 | 113 | Line Dubins::getCenter() const { 114 | return Line(getFirstArc().getEnd(), len2); 115 | } 116 | 117 | Arc Dubins::getCenterArc() const { 118 | State p = getFirstArc().getEnd(); 119 | return Arc(p, len2, radius); 120 | } 121 | 122 | Arc Dubins::getSecondArc() const { 123 | State st; 124 | if (isCCC) { 125 | st = getCenterArc().getEnd(); 126 | } else { 127 | Point ps = getCenter().p2; 128 | st = State(ps, start.ang + len1); 129 | } 130 | return Arc(st, len3, radius); 131 | } 132 | 133 | StateAtDistance Dubins::intersectLine(const Line &line) const { 134 | 135 | StateAtDistance p = getFirstArc().intersectionPoint(line); 136 | if (!p.state.invalid()) { 137 | return p; 138 | } 139 | 140 | if (isCCC) { 141 | p = getCenterArc().intersectionPoint(line); 142 | if (!p.state.invalid()) { 143 | return p; 144 | } 145 | } else { 146 | p = getCenter().intersectionPoint(line); 147 | if (!p.state.invalid()) { 148 | return p; 149 | } 150 | } 151 | 152 | p = getSecondArc().intersectionPoint(line); 153 | if (!p.state.invalid()) { 154 | return p; 155 | } 156 | 157 | return StateAtDistance(State(), 0); 158 | } 159 | 160 | // Find the first intrsection between Dubins and Circle 161 | // If no intersection is detected non-valid State is returned 162 | // Validity of state can be tested by Dubins::isValid() function 163 | // Returned StateAtDistance type also contains information about distance 164 | // in which intersection occurs. 165 | StateAtDistance Dubins::intersectCircle(const Circle &circle) const { 166 | State state; 167 | double len; 168 | tie(state, len) = circle.firstIntersection(getFirstArc()); 169 | if (state.isValid() && len < getFirstArc().getLength()) { 170 | return StateAtDistance(state, len); 171 | } 172 | 173 | if (isCCC) { 174 | tie(state, len) = circle.firstIntersection(getCenterArc()); 175 | if (!state.invalid() && len < getCenterArc().getLength()) { 176 | return StateAtDistance(state, len + getFirstArc().getLength()); 177 | } 178 | } else { 179 | Line center_segment = getCenter(); 180 | Point candidate = circle.halfLineIntersection(getFirstArc().getEnd()); 181 | if (candidate.isValid()){ 182 | double dist = candidate.distance(center_segment.getStart().point); 183 | if(dist <= center_segment.getLength()){ 184 | dist += getFirstArc().getLength(); 185 | return StateAtDistance(getState(dist), dist); 186 | } 187 | } 188 | } 189 | 190 | tie(state, len) = circle.firstIntersection(getSecondArc()); 191 | if (state.isValid() && len < getSecondArc().getLength()) { 192 | double totalLength = getFirstArc().getLength(); 193 | if (isCCC) { 194 | totalLength += getCenterArc().getLength(); 195 | } else { 196 | totalLength += getCenter().getLength(); 197 | } 198 | totalLength += len; 199 | return StateAtDistance(state, totalLength); 200 | } 201 | 202 | return StateAtDistance(State::getInvalid(), NAN); 203 | } 204 | 205 | bool Dubins::intersectLineBool(const Line &line) const { 206 | if (!getFirstArc().intersectionPoint(line).state.invalid()) { 207 | return true; 208 | } 209 | 210 | if (isCCC) { 211 | if (!getCenterArc().intersectionPoint(line).state.invalid()) { 212 | return true; 213 | } 214 | } else { 215 | if (!getCenter().intersectionPoint(line).state.invalid()) { 216 | return true; 217 | } 218 | } 219 | 220 | if (!getSecondArc().intersectionPoint(line).state.invalid()) { 221 | return true; 222 | } 223 | 224 | return false; 225 | } 226 | 227 | ostream &operator<<(ostream &os, const Dubins &d) { 228 | os << setprecision(5); 229 | os << "Dubins maneuver " << d.start << " --> " << d.end << endl 230 | << (d.isCCC ? "CCC" : "CSC") << " type " << d.getType() 231 | << "\tlen = " << d.getLength() 232 | << "\t(" 233 | << "n1 = " << d.getLen1() 234 | << ", n2 = " << d.getLen2() 235 | << ", n3 = " << d.getLen3() 236 | << ")"; 237 | 238 | return os; 239 | } 240 | } 241 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/dubins.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dubins.h 3 | * @author Petr Vana 4 | * @brief Header file for Dubins maneuver 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "dtype.h" 10 | #include "line.h" 11 | #include "arc.h" 12 | #include "angleinterval.h" 13 | #include "circle.h" 14 | 15 | #define USE_XpXp 0 16 | 17 | namespace opendubins { 18 | 19 | class Dubins : public Path { 20 | 21 | private: 22 | // in radians (positive - left, negative - right) or in meters (straight) 23 | double len1, len2, len3; 24 | 25 | DType type; 26 | 27 | public: 28 | State start; 29 | State end; 30 | 31 | double radius; 32 | 33 | bool isCCC; 34 | 35 | double length; 36 | 37 | // default constructor 38 | Dubins(); 39 | 40 | // create Dubins from a straight line (RADIUS = 1) 41 | Dubins(const Line& line); 42 | 43 | // classical Dubins maneuver 44 | Dubins(const State& from, const State& to, const double& radius); 45 | 46 | // Dubins Interval Problem (DIP) 47 | Dubins(AngleInterval from, AngleInterval to, double radius); 48 | 49 | // Generalited Dubins Interval Problem (GDIP) 50 | // diff1 - neighborhood around the start state 51 | // diff2 - neighborhood around the end state 52 | Dubins(AngleInterval from, AngleInterval to, double radius, double diff1, double diff2); 53 | 54 | // Generalited Dubins Interval Problem (GDIP) 55 | // diff - neighborhood around the end state 56 | Dubins(AngleInterval from, AngleInterval to, double radius, double diff2); 57 | 58 | Dubins(State pos, bool isCCC, double le1, double le2, double le3, double radius); 59 | 60 | virtual ~Dubins(); 61 | 62 | bool check(); 63 | 64 | virtual State getState(double len) const; 65 | 66 | virtual StateAtDistance getClosestStateAndDistance(const Point &p) const; 67 | 68 | Arc getFirstArc() const; 69 | 70 | Line getCenter() const; 71 | 72 | Arc getCenterArc() const; 73 | 74 | Arc getSecondArc() const; 75 | 76 | StateAtDistance intersectLine(const Line &line) const; 77 | 78 | StateAtDistance intersectCircle(const Circle &circle) const; 79 | 80 | bool intersectLineBool(const Line &line) const; 81 | 82 | inline double getLength() const { 83 | return length; 84 | } 85 | 86 | inline bool getIsCCC() const { 87 | return isCCC; 88 | } 89 | 90 | inline double getLen1() const { 91 | return len1; 92 | } 93 | 94 | inline double getLen2() const { 95 | return len2; 96 | } 97 | 98 | inline double getLen3() const { 99 | return len3; 100 | } 101 | 102 | inline double getRadius() const { 103 | return radius; 104 | } 105 | 106 | inline void calculateLength() { 107 | if (isCCC) { 108 | length = radius * (std::fabs(len1) + std::fabs(len2) + std::fabs(len3)); 109 | } else { 110 | length = len2 + radius * (std::fabs(len1) + std::fabs(len3)); 111 | } 112 | } 113 | 114 | inline double getTotalTurning() { 115 | return std::fabs(len1) + std::fabs(len3); 116 | } 117 | 118 | inline DType getType() const { 119 | return type; 120 | } 121 | 122 | private: 123 | // create standard Dubins maneuver 124 | void init(const State & from, const State & to); 125 | // solve DIP 126 | void initIntervalProblem(const AngleInterval & from, const AngleInterval & to); 127 | // solve OS-GDIP 128 | void initGDIP(AngleInterval from, AngleInterval to, double diff); 129 | // convert GDIP to one-side GDIP (OS-GDIP) 130 | void initGDIP(AngleInterval from, AngleInterval to, double diff1, double diff2); 131 | 132 | // private functions for solving DIP ------------------ 133 | 134 | // create standard Dubins maneuver for DIP using pre-computed values 135 | void initForDIP(const State & from, const State & to, const Vector & dir1, const Vector & dir2); 136 | 137 | // private functions for solving GDIP ------------------ 138 | void GDIP_S(const AngleInterval& from, const AngleInterval& to, const double &diff2); 139 | void GDIP_S_onEdge(const AngleInterval& from, const Point& to, const double& diff2, double dir); 140 | 141 | void GDIP_CS(const State& from, const AngleInterval& to, const double &diff2, bool isLeft); 142 | void GDIP_CS_onEdge(const State& from, const AngleInterval& to, const double& diff2, double dir, const bool& isLeft); 143 | 144 | void GDIP_SC(const AngleInterval& from, const AngleInterval& to, const double& diff2); 145 | 146 | void GDIP_CSC_same_side(const State &from, const State &to, const double &diff2, const bool &isLeft); 147 | void GDIP_CSC_diff_side(const State &from, const State &to, const double &diff2, const bool &isLeft); 148 | 149 | void GDIP_Lp_Rp(const AngleInterval& from, const AngleInterval& to, const double &diff2); 150 | 151 | void GDIP_CCp(const State& from, const AngleInterval& to, const double &diff2, bool isLeft); 152 | void GDIP_CpC(const AngleInterval& from, const State& to, const double &diff2, bool isLeft); 153 | 154 | void GDIP_CCC(const State &from, const State &to, const double &diff2, const bool &isLeft); 155 | 156 | static void Dubins_StateToCircle(const State& from, const Circle& goal, const double& radius, const bool& isLeft, 157 | double& turn, double& straight, double& length); 158 | 159 | }; 160 | 161 | std::ostream &operator<<(std::ostream &os, const Dubins &d); 162 | 163 | } 164 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/dubinsmaneuver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dubinsmaneuver.cpp 3 | * @author Petr Vana 4 | * @brief Implementation of 2D Dubins maneuver, all 6 maneuver types. 5 | */ 6 | 7 | #include "dubins.h" 8 | 9 | namespace opendubins { 10 | 11 | Dubins::Dubins(const State& newStart, const State& newEnd, const double& newRadius) { 12 | radius = newRadius; 13 | length = std::numeric_limits::max(); 14 | type = DType::Unknown; 15 | init(newStart, newEnd); 16 | } 17 | 18 | void Dubins::init(const State & from, const State & to) { 19 | bool used = false; 20 | 21 | Vector dir1 = from.getNormalizedDirection(); 22 | Vector dir2 = to.getNormalizedDirection(); 23 | 24 | Vector dir1radius = dir1 * radius; 25 | Vector dir2radius = dir2 * radius; 26 | 27 | Point c1left = from.point + dir1radius.left(); 28 | Point c1right = from.point + dir1radius.right(); 29 | 30 | Point c2left = to.point + dir2radius.left(); 31 | Point c2right = to.point + dir2radius.right(); 32 | 33 | double n1, n2, n3, nLength, centerDistance; 34 | 35 | // RSR - maneuver 36 | 37 | Vector diff = c2right - c1right; 38 | double diffLen = diff.length(); 39 | 40 | if (diffLen < radius * TOLERANCE) { 41 | n1 = angleToRight(from.ang, to.ang); 42 | n2 = 0; 43 | n3 = 0; 44 | } else { 45 | double ang = diff.getAngle(); 46 | 47 | n1 = angleToRight(from.ang, ang); 48 | n2 = diffLen; 49 | n3 = angleToRight(ang, to.ang); 50 | 51 | n1 = checkToleranceRight(n1); 52 | n3 = checkToleranceRight(n3); 53 | } 54 | 55 | nLength = n2 + radius * (std::fabs(n1) + std::fabs(n3)); 56 | 57 | if (nLength < length) { 58 | len1 = n1; 59 | len2 = n2; 60 | len3 = n3; 61 | length = nLength; 62 | used = true; 63 | isCCC = false; 64 | type = DType::RSR; 65 | } 66 | 67 | // LSL - maneuver 68 | 69 | diff = c2left - c1left; 70 | diffLen = diff.length(); 71 | 72 | if (diffLen < radius * TOLERANCE) { 73 | n1 = angleToLeft(from.ang, to.ang); 74 | n2 = 0; 75 | n3 = 0; 76 | } else { 77 | double ang = diff.getAngle(); 78 | 79 | n1 = angleToLeft(from.ang, ang); 80 | n2 = diffLen; 81 | n3 = angleToLeft(ang, to.ang); 82 | 83 | n1 = checkToleranceLeft(n1); 84 | n3 = checkToleranceLeft(n3); 85 | 86 | } 87 | 88 | nLength = n2 + radius * (std::fabs(n1) + std::fabs(n3)); 89 | 90 | if (nLength < length) { 91 | len1 = n1; 92 | len2 = n2; 93 | len3 = n3; 94 | length = nLength; 95 | used = true; 96 | isCCC = false; 97 | type = DType::LSL; 98 | } 99 | 100 | // LSR - maneuver 101 | 102 | diff = c2right - c1left; 103 | centerDistance = diff.length(); 104 | 105 | if (centerDistance * (1.0) >= 2 * radius) { 106 | double alpha = std::asin(std::fmin(1, 2 * radius / centerDistance)); 107 | double centerAngle = std::atan2(c2right.y - c1left.y, c2right.x - c1left.x) + alpha; 108 | n2 = std::sqrt(std::fmax(0, centerDistance * centerDistance - 4 * radius * radius)); 109 | 110 | // normalize angle 111 | n1 = angleToLeft(from.ang, centerAngle); 112 | n3 = angleToRight(centerAngle, to.ang); 113 | 114 | n1 = checkToleranceLeft(n1); 115 | n3 = checkToleranceRight(n3); 116 | 117 | nLength = n2 + radius * (std::fabs(n1) + std::fabs(n3)); 118 | 119 | if (nLength < length) { 120 | len1 = n1; 121 | len2 = n2; 122 | len3 = n3; 123 | length = nLength; 124 | used = true; 125 | isCCC = false; 126 | type = DType::LSR; 127 | } 128 | } 129 | 130 | // RSL - maneuver 131 | 132 | diff = c2left - c1right; 133 | centerDistance = diff.length(); 134 | 135 | if (centerDistance * (1.0) >= 2 * radius) { 136 | double alpha = std::asin(std::fmin(1, 2 * radius / centerDistance)); 137 | double centerAngle = std::atan2(c2left.y - c1right.y, c2left.x - c1right.x) - alpha; 138 | n2 = std::sqrt(std::fmax(0, centerDistance * centerDistance - 4 * radius * radius)); 139 | 140 | // normalize angle 141 | n1 = angleToRight(from.ang, centerAngle); 142 | n3 = angleToLeft(centerAngle, to.ang); 143 | 144 | n1 = checkToleranceRight(n1); 145 | n3 = checkToleranceLeft(n3); 146 | 147 | nLength = n2 + radius * (std::fabs(n1) + std::fabs(n3)); 148 | 149 | if (nLength < length) { 150 | len1 = n1; 151 | len2 = n2; 152 | len3 = n3; 153 | length = nLength; 154 | used = true; 155 | isCCC = false; 156 | type = DType::RSL; 157 | } 158 | } 159 | 160 | // CCC maneuver is possible only in case start and end state is close enougth 161 | if ((from.point - to.point).length() <= 4 * radius) { 162 | 163 | // RLR - maneuver 164 | diff = c2right - c1right; 165 | centerDistance = diff.length(); 166 | 167 | if (centerDistance <= 4 * radius) { 168 | // direction of Vector(S1,S2) to Vector(S1,S3) 169 | double alpha = std::acos(centerDistance / radius / 4); 170 | 171 | // direction between first and second arc 172 | double dir12 = diff.getAngle() - M_PI / 2 - alpha; 173 | // direction between second and third arc 174 | double dir23 = diff.getAngle() + M_PI / 2 + alpha; 175 | 176 | n1 = angleToRight(from.ang, dir12); 177 | n2 = angleToLeft(dir12, dir23); 178 | n3 = angleToRight(dir23, to.ang); 179 | 180 | nLength = radius * (std::fabs(n1) + std::fabs(n2) + std::fabs(n3)); 181 | 182 | if (nLength < length) { 183 | isCCC = true; 184 | len1 = n1; 185 | len2 = n2; 186 | len3 = n3; 187 | length = nLength; 188 | used = true; 189 | type = DType::RLR; 190 | } 191 | } 192 | 193 | // LRL - maneuver 194 | diff = c2left - c1left; 195 | centerDistance = diff.length(); 196 | 197 | if (centerDistance <= 4 * radius) { 198 | // direction of Vector(S1,S2) to Vector(S1,S3) 199 | double alpha = std::acos(centerDistance / radius / 4); 200 | 201 | // direction between first and second arc 202 | double dir12 = diff.getAngle() + M_PI / 2 + alpha; 203 | // direction between second and third arc 204 | double dir23 = diff.getAngle() - M_PI / 2 - alpha; 205 | 206 | n1 = angleToLeft(from.ang, dir12); 207 | n2 = angleToRight(dir12, dir23); 208 | n3 = angleToLeft(dir23, to.ang); 209 | 210 | nLength = radius * (std::fabs(n1) + std::fabs(n2) + std::fabs(n3)); 211 | 212 | if (nLength < length) { 213 | isCCC = true; 214 | len1 = n1; 215 | len2 = n2; 216 | len3 = n3; 217 | length = nLength; 218 | used = true; 219 | type = DType::LRL; 220 | } 221 | } 222 | } 223 | 224 | if (used) { 225 | start = from; 226 | end = to; 227 | } 228 | } 229 | 230 | } 231 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/dubinspath.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dubinspath.cpp 3 | * @author Petr Vana 4 | * @brief This class connects multiple Dubins maneuvers together to enable uniform sampling and collision detection. 5 | */ 6 | 7 | #include "dubinspath.h" 8 | 9 | namespace opendubins { 10 | 11 | DubinsPath::DubinsPath() {} 12 | 13 | DubinsPath::DubinsPath(std::vector path) { 14 | this->path = path; 15 | } 16 | 17 | State DubinsPath::getState(double len) const { 18 | double sum = 0; 19 | for (auto & d: path) { 20 | double diff = len - sum; 21 | if(diff < d.length){ 22 | return d.getState(diff); 23 | } 24 | sum += d.length; 25 | } 26 | return path.back().getEnd(); 27 | } 28 | 29 | double DubinsPath::getLength() const{ 30 | double sum = 0; 31 | for (auto & d: path) { 32 | sum += d.length; 33 | } 34 | return sum; 35 | } 36 | 37 | bool DubinsPath::intersect(Line line) const{ 38 | for (auto & d: path) { 39 | if(d.intersectLineBool(line)){ 40 | return true; 41 | } 42 | } 43 | return false; 44 | } 45 | 46 | StateAtDistance DubinsPath::getClosestStateAndDistance(const Point &p) const { 47 | StateAtDistance closest; 48 | // compute distance of all previous maneuvers 49 | double distance = 0; 50 | // iterate over all maneuvers 51 | for(auto & d : path){ 52 | auto state = d.getClosestStateAndDistance(p); 53 | if(!closest.isValid() || closest.distance > state.distance){ 54 | closest = state; 55 | closest.distance += distance; 56 | } 57 | distance += d.getLength(); 58 | } 59 | return closest; 60 | } 61 | 62 | StateAtDistance DubinsPath::intersectionPoint(Line line) const{ 63 | for(auto d : path){ 64 | auto intersection = d.intersectLine(line); 65 | if(intersection.isValid()){ 66 | return intersection; 67 | } 68 | } 69 | return StateAtDistance(); 70 | } 71 | 72 | std::vector DubinsPath::interpolate(double step) const { 73 | std::vector pnts; 74 | 75 | double speed = 0; 76 | 77 | for(double distance = 0; distance < getLength(); distance += speed){ 78 | pnts.push_back(getState(distance).point); 79 | speed += step * 0.1; 80 | if(speed > step){ 81 | speed = step; 82 | } 83 | } 84 | 85 | return pnts; 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/dubinspath.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dubinspath.h 3 | * @author Petr Vana 4 | * @brief This class connects multiple Dubins maneuvers together to enable uniform sampling and collision detection. 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "path.h" 10 | #include "dubins.h" 11 | #include "stateatdistance.h" 12 | 13 | namespace opendubins { 14 | 15 | class DubinsPath : public Path { 16 | public: 17 | 18 | DubinsPath(); 19 | DubinsPath(std::vector path); 20 | 21 | std::vector path; 22 | 23 | State getState(double len) const; 24 | double getLength() const; 25 | bool intersect(Line line) const; 26 | 27 | StateAtDistance getClosestStateAndDistance(const Point &p) const; 28 | StateAtDistance intersectionPoint(Line line) const; 29 | 30 | std::vector interpolate(double step) const; 31 | 32 | inline void add(const Dubins & d) { 33 | path.push_back(d); 34 | } 35 | 36 | }; 37 | 38 | inline std::ostream &operator<<(std::ostream &os, const DubinsPath &d) { 39 | for(auto & maneuver : d.path) { 40 | os << "Dubins maneuver " << maneuver.start << " to " << maneuver.end; 41 | } 42 | return os; 43 | } 44 | 45 | } 46 | 47 | 48 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/line.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file line.cpp 3 | * @author Petr Vana 4 | * @brief Line is a basic segment in Dubins maneuver 5 | */ 6 | 7 | #include "line.h" 8 | 9 | namespace opendubins { 10 | 11 | Line::Line(const Point &np1, const Point &np2) { 12 | p1 = np1; 13 | p2 = np2; 14 | } 15 | 16 | Line::Line(State pos, double len) { 17 | p1 = pos.point; 18 | p2 = pos.point + pos.getNormalizedDirection() * len; 19 | } 20 | 21 | State Line::getState(double len) const { 22 | Vector dir = getDirection(); 23 | Point p = p1 + dir.normalize() * len; 24 | if(dir.dx == 0 && dir.dy == 0){ 25 | return State(p1, 0); 26 | } 27 | return State(p, dir.getAngle()); 28 | } 29 | 30 | StateAtDistance Line::getClosestStateAndDistance(const Point &p) const { 31 | StateAtDistance ret; 32 | 33 | auto len = getLength(); 34 | if (len < TOLERANCE) { 35 | ret.state = State(p1, 0); 36 | ret.distance = 0; 37 | } else { 38 | auto dir = (p2 - p1).normalize(); 39 | 40 | auto usedLen = (p - p1).dotProduct(dir); 41 | if (usedLen < 0) { 42 | ret.state = getStart(); 43 | ret.distance = 0; 44 | } else if (usedLen > len) { 45 | ret.state = getEnd(); 46 | ret.distance = getLength(); 47 | } else { 48 | ret.state = getState(usedLen); 49 | ret.distance = usedLen; 50 | } 51 | } 52 | 53 | return ret; 54 | } 55 | 56 | bool Line::intersect(Line line) const { 57 | // normales to directions 58 | Vector norm1 = this->getDirection().left().normalize(); 59 | Vector norm2 = line.getDirection().left().normalize(); 60 | 61 | // end point distances from line1 62 | double d11 = (line.p1 - this->p1).dotProduct(norm1); 63 | double d12 = (line.p2 - this->p1).dotProduct(norm1); 64 | 65 | // end point distances from line2 66 | double d21 = (this->p1 - line.p1).dotProduct(norm2); 67 | double d22 = (this->p2 - line.p1).dotProduct(norm2); 68 | 69 | // end points of first line are at opposite sides of second line 70 | if (d11 * d12 < 0 && d21 * d22 < 0) { 71 | return true; 72 | } 73 | 74 | return false; 75 | } 76 | 77 | StateAtDistance Line::intersectionPoint(Line line) const { 78 | 79 | if (intersect(line)) { 80 | Vector dir2 = line.getDirection(); 81 | Vector normal2 = dir2.normalize().left(); 82 | 83 | double dist = normal2.dotProduct(p1 - line.p1); 84 | double speed = -normal2.dotProduct(getDirection().normalize()); 85 | 86 | if (dist * speed > 0) { 87 | dist = dist / speed; 88 | 89 | Point inter = p1 + getDirection().normalize() * dist; 90 | return StateAtDistance(State(inter, getDirection().getAngle()), dist); 91 | // TODO 92 | //return Intersection(Position(Point(0,0), 0), 0); 93 | } 94 | } 95 | 96 | return StateAtDistance(State(), 0); 97 | } 98 | 99 | } 100 | 101 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/line.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file line.h 3 | * @author Petr Vana 4 | * @brief Line is a basic segment in Dubins maneuver 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "path.h" 10 | #include "stateatdistance.h" 11 | 12 | namespace opendubins { 13 | 14 | class Line : public Path { 15 | public: 16 | 17 | Point p1; 18 | Point p2; 19 | 20 | inline Line() { }; 21 | 22 | Line(const Point &, const Point &); 23 | 24 | Line(State, double); 25 | 26 | virtual State getState(double len) const; 27 | 28 | virtual StateAtDistance getClosestStateAndDistance(const Point &p) const; 29 | 30 | inline double getLength() const { 31 | return (p2 - p1).length(); 32 | } 33 | 34 | inline Vector getDirection() const { 35 | return p2 - p1; 36 | } 37 | 38 | bool intersect(Line line) const; 39 | 40 | StateAtDistance intersectionPoint(Line line) const; 41 | 42 | }; 43 | 44 | inline std::ostream &operator<<(std::ostream &os, const Line &d) { 45 | os << "Line from " << d.p1 << " to " << d.p2; 46 | return os; 47 | } 48 | 49 | } 50 | 51 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/path.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file path.cpp 3 | * @author Petr Vana 4 | * @brief Path is an abstract class for Line and Arc which are basic segments in Dubins maneuver. 5 | */ 6 | 7 | #include "path.h" 8 | 9 | namespace opendubins { 10 | 11 | Path::Path() { } 12 | 13 | Path::~Path() { } 14 | 15 | State Path::getStart() const { 16 | return getState(0); 17 | } 18 | 19 | State Path::getEnd() const { 20 | return getState(getLength()); 21 | } 22 | 23 | State Path::getClosestState(const Point &p) const { 24 | return getClosestStateAndDistance(p).state; 25 | } 26 | 27 | } 28 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/path.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file path.h 3 | * @author Petr Vana 4 | * @brief Path is an abstract class for Line and Arc which are basic segments in Dubins maneuver. 5 | */ 6 | 7 | 8 | #pragma once 9 | 10 | #include "state.h" 11 | #include "stateatdistance.h" 12 | 13 | namespace opendubins { 14 | 15 | class Path { 16 | public: 17 | Path(); 18 | 19 | virtual ~Path(); 20 | 21 | public: 22 | virtual double getLength() const = 0; 23 | 24 | virtual State getState(double len) const = 0; 25 | 26 | virtual State getStart() const; 27 | 28 | virtual State getEnd() const; 29 | 30 | /** 31 | * Get the closest state to point p on the path according to the Euclidean metric. 32 | * It invokes function getClosestIntersection(const Point &p) and return only state. 33 | */ 34 | virtual State getClosestState(const Point &p) const; 35 | 36 | /** 37 | * Find the closest state to point p on the path according to the euclidean metric. 38 | * It return Intersection (state and distance). 39 | */ 40 | virtual StateAtDistance getClosestStateAndDistance(const Point &p) const = 0; 41 | }; 42 | 43 | } 44 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/point.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file point.h 3 | * @author Petr Vana 4 | * @brief Point in the 2D plane. 5 | */ 6 | 7 | 8 | #pragma once 9 | 10 | #include "vector.h" 11 | 12 | namespace opendubins { 13 | 14 | class Point { 15 | public: 16 | 17 | double x; 18 | double y; 19 | 20 | Point() { 21 | x = 0; 22 | y = 0; 23 | } 24 | 25 | Point(double x, double y) { 26 | this->x = x; 27 | this->y = y; 28 | } 29 | 30 | inline double getX() const { 31 | return x; 32 | } 33 | 34 | inline double getY() const { 35 | return y; 36 | } 37 | 38 | static Point getInvalid(){ 39 | return Point(GDIP_NAN, GDIP_NAN); 40 | } 41 | 42 | inline bool isValid() const{ 43 | return (!std::isnan(x)) && (!std::isnan(y)); 44 | } 45 | 46 | inline Vector operator-(const Point &r) const { 47 | return Vector(x - r.x, y - r.y); 48 | } 49 | 50 | inline Point operator+(const Vector &right) const { 51 | return Point(x + right.dx, y + right.dy); 52 | } 53 | 54 | inline Point operator-(const Vector &right) const { 55 | return Point(x - right.dx, y - right.dy); 56 | } 57 | 58 | inline Point &operator+=(const Vector &rhs) { 59 | this->x += rhs.dx; 60 | this->y += rhs.dy; 61 | return *this; 62 | } 63 | 64 | inline Point &operator+=(const Point &rhs) { 65 | this->x += rhs.x; 66 | this->y += rhs.y; 67 | return *this; 68 | } 69 | 70 | inline Point &operator-=(const Vector &rhs) { 71 | this->x -= rhs.dx; 72 | this->y -= rhs.dy; 73 | return *this; 74 | } 75 | 76 | inline Point &operator-=(const Point &rhs) { 77 | this->x -= rhs.x; 78 | this->y -= rhs.y; 79 | return *this; 80 | } 81 | 82 | inline Point operator+(const Point &right) const { 83 | return Point(x + right.x, y + right.y); 84 | } 85 | 86 | inline Point operator/(const double div) const { 87 | if (div != 0) 88 | return Point(x / div, y / div); 89 | else 90 | return Point(); 91 | } 92 | 93 | inline double distance(const Point &to) const { 94 | return (*this - to).length(); 95 | } 96 | 97 | inline double distanceSquared(const Point &to) const { 98 | return (*this - to).lengthSquared(); 99 | } 100 | 101 | inline Point interpolate(const Point &p, double &alpha) const { 102 | double beta = 1 - alpha; 103 | return Point(beta * x + alpha * p.x, beta * y + alpha * p.y); 104 | } 105 | 106 | inline bool operator==(const Point &b) const { 107 | return (x == b.x) && (y == b.y); 108 | } 109 | 110 | inline size_t hash() const { 111 | auto h1 = std::hash()(x); 112 | auto h2 = std::hash()(y); 113 | return h1 ^ (h2 << 1); 114 | } 115 | }; 116 | 117 | inline std::ostream &operator<<(std::ostream &os, const Point &d) { 118 | os << "Point: (" << d.x << ", " << d.y << ")"; 119 | return os; 120 | } 121 | 122 | } 123 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/state.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file state.cpp 3 | * @author Petr Vana 4 | * @brief State of the Dubins vehicle (position (x,y) and heading angle \theta) 5 | */ 6 | 7 | #include "state.h" 8 | 9 | using namespace std; 10 | 11 | namespace opendubins { 12 | 13 | State::State() { 14 | this->point = Point(0, 0); 15 | this->ang = GDIP_NAN; 16 | } 17 | 18 | State::State(Point point, double angle) { 19 | this->point = point; 20 | this->ang = angle; 21 | } 22 | 23 | State::State(double x, double y, double angle) { 24 | this->point = Point(x, y); 25 | this->ang = angle; 26 | } 27 | 28 | void State::random(double interval) { 29 | this->point = Point(myRandom() * interval, myRandom() * interval); 30 | this->ang = myRandom() * 2 * M_PI; 31 | } 32 | 33 | Vector State::getNormalizedDirection() const { 34 | return Vector(cos(ang), sin(ang)); 35 | } 36 | 37 | ostream &operator<<(ostream &os, const State &pos) { 38 | Point p = pos.getPoint(); 39 | os << "(" << p.x << ", " << p.y << ", " << pos.getAngle() << ")"; 40 | return os; 41 | } 42 | 43 | } 44 | 45 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/state.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file state.h 3 | * @author Petr Vana 4 | * @brief State of the Dubins vehicle (position (x,y) and heading angle \theta) 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "point.h" 10 | 11 | namespace opendubins { 12 | 13 | class State final { 14 | public: 15 | Point point; 16 | double ang; 17 | 18 | State(); 19 | 20 | State(Point, double); 21 | 22 | State(double, double, double); 23 | 24 | inline void set(double x, double y, double angle) { 25 | point.x = x; 26 | point.y = y; 27 | ang = angle; 28 | } 29 | 30 | void random(double); 31 | 32 | Vector getNormalizedDirection() const; 33 | 34 | inline bool invalid() { 35 | return std::isnan(ang); 36 | } 37 | 38 | static State getInvalid() { 39 | return State(Point::getInvalid(), GDIP_NAN); 40 | } 41 | 42 | inline bool isValid() const { 43 | return (!std::isnan(ang)) && point.isValid(); 44 | } 45 | 46 | inline Point getPoint() const { return point; } 47 | 48 | inline double getAngle() const { return ang; } 49 | 50 | inline double squared_distance(const State &p) const { 51 | return point.distanceSquared(p.point); 52 | } 53 | 54 | inline State reverse() const{ 55 | State st = *this; 56 | st.ang = angleToLeft(0, ang + M_PI); 57 | return st; 58 | } 59 | 60 | }; 61 | 62 | std::ostream &operator<<(std::ostream &os, const State &d); 63 | 64 | } 65 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/stateatdistance.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file stateatdistance.h 3 | * @author Petr Vana 4 | * @brief Auxiliary structure for returning informations about intersections. 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "state.h" 10 | 11 | namespace opendubins { 12 | 13 | struct StateAtDistance { 14 | 15 | State state; 16 | double distance; 17 | 18 | StateAtDistance() { 19 | state = State(Point(GDIP_NAN, GDIP_NAN), GDIP_NAN); 20 | distance = GDIP_NAN; 21 | } 22 | 23 | StateAtDistance(State state, double distance) { 24 | this->state = state; 25 | this->distance = distance; 26 | } 27 | 28 | bool isValid() const { 29 | return ! std::isnan(distance); 30 | } 31 | 32 | }; 33 | 34 | } 35 | -------------------------------------------------------------------------------- /gdip/core/src/opendubins/vector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file vector.h 3 | * @author Petr Vana 4 | * @brief Vector in the 2D plane. 5 | */ 6 | 7 | #pragma once 8 | 9 | #include "dmath.h" 10 | 11 | namespace opendubins { 12 | 13 | class Vector { 14 | public: 15 | 16 | double dx, dy; 17 | 18 | Vector(double x, double y) : 19 | dx(x), dy(y) { 20 | } 21 | 22 | Vector(double angle) : 23 | dx(std::cos(angle)), dy(std::sin(angle)) { 24 | } 25 | 26 | inline double getAngle() const { 27 | return std::atan2(dy, dx); 28 | } 29 | 30 | static Vector getInvalid(){ 31 | return Vector(GDIP_NAN, GDIP_NAN); 32 | } 33 | 34 | inline bool isValid() const{ 35 | return (!std::isnan(dx)) && (!std::isnan(dy)); 36 | } 37 | 38 | inline Vector right() const { 39 | return Vector(dy, -dx); 40 | } 41 | 42 | inline Vector left() const { 43 | return Vector(-dy, dx); 44 | } 45 | 46 | inline Vector negate() const { 47 | return Vector(-dx, -dy); 48 | } 49 | 50 | inline double length() const { 51 | return std::sqrt(dx * dx + dy * dy); 52 | } 53 | 54 | inline double lengthSquared() const { 55 | return dx * dx + dy * dy; 56 | } 57 | 58 | inline Vector normalize() const { 59 | double len = this->length(); 60 | return Vector(dx / len, dy / len); 61 | } 62 | 63 | inline double dotProduct(const Vector &vec) const { 64 | return dx * vec.dx + dy * vec.dy; 65 | } 66 | 67 | inline Vector &operator*=(const double &mult) { 68 | dx *= mult; 69 | dy *= mult; 70 | return *this; 71 | } 72 | 73 | inline Vector &operator+=(const Vector &rght) { 74 | dx += rght.dx; 75 | dy += rght.dy; 76 | return *this; 77 | } 78 | 79 | inline Vector rotate(const double &angle) const { 80 | const double c = std::cos(angle); 81 | const double s = std::sin(angle); 82 | return Vector(c * dx - s * dy, c * dy + s * dx); 83 | } 84 | 85 | }; 86 | 87 | inline Vector operator+(const Vector &left, const Vector &right) { 88 | return Vector(left.dx + right.dx, left.dy + right.dy); 89 | } 90 | 91 | inline Vector operator*(const Vector &left, const double &right) { 92 | return Vector(left.dx * right, left.dy * right); 93 | } 94 | 95 | inline Vector operator*(const double &left, const Vector &right) { 96 | return Vector(right.dx * left, right.dy * left); 97 | } 98 | 99 | } 100 | -------------------------------------------------------------------------------- /gdip/core/test/angleintervaltest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file angleintervaltest.h 3 | * @author Petr Vana 4 | * @brief Test merging of multiple angle intervals from special Euclidean space S 5 | */ 6 | 7 | #include "gtest/gtest.h" 8 | #include "opendubins/angleinterval.h" 9 | 10 | using namespace std; 11 | using namespace opendubins; 12 | 13 | namespace angleintervaltest { 14 | 15 | const int COUNT = 10000; 16 | 17 | int ins(vector& ints, double angle){ 18 | int ret = 0; 19 | for(auto& i : ints){ 20 | if(i.inInterval(angle)){ 21 | ret++; 22 | } 23 | } 24 | return ret; 25 | } 26 | 27 | TEST (AngleIntervalTest, Merge) { 28 | 29 | const int COUNT_RND = 10; 30 | 31 | for (int i = 0; i < COUNT; ++i) { 32 | vector intervals; 33 | 34 | for(int j = 0; j < 3; ++j){ 35 | auto a = AngleInterval(Point(), myRandom()*2*M_PI, myRandom()*2*M_PI); 36 | intervals.push_back(a); 37 | } 38 | 39 | auto merged = AngleInterval::mergeIntervals(intervals); 40 | 41 | for(int j = 0; j < COUNT_RND; ++j){ 42 | double angle = 2 * M_PI * myRandom(); 43 | 44 | if(ins(intervals, angle) > 0){ 45 | EXPECT_EQ(1, ins(merged, angle)) << "Should intersect angle = " << angle; 46 | }else{ 47 | EXPECT_EQ(0, ins(merged, angle)); 48 | } 49 | } 50 | } 51 | } 52 | 53 | } 54 | -------------------------------------------------------------------------------- /gdip/core/test/circletest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file circletest.h 3 | * @author Petr Vana 4 | * @brief Test implementation for intersections with cirecle 5 | */ 6 | 7 | #include "gtest/gtest.h" 8 | #include "opendubins/arc.h" 9 | #include "opendubins/circle.h" 10 | 11 | using namespace std; 12 | using namespace opendubins; 13 | 14 | namespace circletest { 15 | 16 | const int COUNT = 1000; 17 | 18 | void check(Circle& c, Arc& arc){ 19 | State pr; 20 | double len; 21 | tie(pr, len) = c.firstIntersection(arc); 22 | 23 | Point pr2 = arc.getState(len).point; 24 | 25 | EXPECT_TRUE(pr.point.distance(pr2) < TOLERANCE) << "To far"; 26 | EXPECT_TRUE(fabs(c.center.distance(pr.point) - c.radius) < c.radius * TOLERANCE); 27 | } 28 | 29 | 30 | TEST (CircleTest, ArcIntersectionRight) { 31 | Circle c(Point(0,0), 0.9); 32 | Arc arc(State(-1,-1,M_PI), -2*M_PI, 1); 33 | 34 | check(c, arc); 35 | } 36 | 37 | TEST (CircleTest, ArcIntersectionLeft) { 38 | Circle c(Point(0,0), 1); 39 | Arc arc(State(1,1,M_PI), 2*M_PI, 1.1); 40 | 41 | check(c, arc); 42 | } 43 | 44 | TEST (CircleTest, ArcIntersectionRandom) { 45 | 46 | for(int i = 0; i < COUNT; ++i) { 47 | double radius = 0.1 + 2*myRandom(); 48 | Circle c(Point(0, 0), radius); 49 | State s; 50 | s.random(2); 51 | 52 | auto a = myRandom(); 53 | auto angle = a > 0.5 ? 2*M_PI : -2*M_PI; 54 | Arc arc(s, angle, 2*myRandom()); 55 | 56 | if (c.center.distance(arc.getCenter()) < c.radius + arc.radius) { 57 | if (c.center.distance(arc.getCenter()) > fabs(c.radius - arc.radius)) { 58 | check(c, arc); 59 | } 60 | } 61 | } 62 | } 63 | 64 | TEST (CircleTest, CircleIntersectionRandom) { 65 | 66 | const int COUNT_INTER = 10; 67 | 68 | for(int i = 0; i < COUNT / COUNT_INTER; ++i) { 69 | double radius1 = 2*myRandom(); 70 | double radius2 = 2*myRandom(); 71 | 72 | State s1, s2; 73 | s1.random(2); 74 | s2.random(2); 75 | 76 | Circle c1(s1.point, radius1); 77 | Circle c2(s2.point, radius2); 78 | 79 | auto intersect = c1.getIntersectionAngleInterval(c2); 80 | 81 | if(intersect.isValid()) { 82 | for (int j = 0; j < COUNT_INTER; ++j) { 83 | double angle = 2 * M_PI * myRandom(); 84 | Point randomPoint = c1.center + Vector(angle) * c1.radius; 85 | 86 | if(intersect.inInterval(angle)){ 87 | EXPECT_LE(randomPoint.distance(c2.center), c2.radius + TOLERANCE) << "FP - Out of the circle"; 88 | }else{ 89 | EXPECT_GE(randomPoint.distance(c2.center), c2.radius - TOLERANCE) << "FN - In the circle"; 90 | } 91 | } 92 | }else{ 93 | double distance = s1.point.distance(s2.point); 94 | if(distance < radius1 + radius2 - TOLERANCE){ 95 | if(radius1 < radius2 || distance > radius1 - radius2) { 96 | EXPECT_TRUE(false) << "There should be some intersection " 97 | << endl << s1.point 98 | << endl << s2.point 99 | << endl << "radii: " << radius1 << ", " << radius2 100 | << endl << distance << endl; 101 | } 102 | } 103 | } 104 | } 105 | } 106 | 107 | TEST (CircleTest, SectorIntersectionValid) { 108 | for(int i = 0; i < COUNT; ++i) { 109 | double radius1 = 2*myRandom(); 110 | 111 | State s1, s2; 112 | s1.random(2); 113 | s2.random(2); 114 | 115 | Circle circle(s1.point, radius1); 116 | AngleInterval sector(s2.point, s2.ang, 0.1); 117 | 118 | auto vec = circle.shortestVectorInSector(sector); 119 | auto intersect = sector.point + vec; 120 | 121 | if (intersect.isValid()) { 122 | Vector diff = intersect - sector.point; 123 | if(diff.length() > TOLERANCE) { 124 | EXPECT_TRUE(sector.inIntervalWithTollerance(diff.getAngle(), TOLERANCE)) 125 | << "Point is out of the sector"; 126 | } else { 127 | EXPECT_LE(diff.length(), std::max(circle.center.distance(sector.point) - circle.radius + TOLERANCE, TOLERANCE)); 128 | } 129 | EXPECT_LE(intersect.distance(circle.center), circle.radius + TOLERANCE) << "Point is out of the circle"; 130 | } 131 | } 132 | } 133 | 134 | TEST (CircleTest, SectorIntersectionRandom) { 135 | 136 | const int COUNT_INTER = 10; 137 | const double M = numeric_limits::max(); 138 | 139 | for(int i = 0; i < COUNT / COUNT_INTER; ++i) { 140 | double radius1 = 2*myRandom(); 141 | 142 | State s1, s2; 143 | s1.random(2); 144 | s2.random(2); 145 | 146 | Circle circle(s1.point, radius1); 147 | AngleInterval sector(s2.point, s2.ang, 0.1); 148 | 149 | auto vec = circle.shortestVectorInSector(sector); 150 | auto intersect = sector.point + vec; 151 | 152 | double len = M; 153 | 154 | for(int j = 0; j < COUNT_INTER; j++){ 155 | double ang = myRandom() * 2 * M_PI; 156 | Point np = circle.center + Vector(ang) * circle.radius; 157 | if(sector.inInterval((np - sector.point).getAngle())){ 158 | len = min(len, np.distance(sector.point)); 159 | } 160 | } 161 | 162 | if (intersect.isValid()) { 163 | EXPECT_LE(intersect.distance(sector.point), len + TOLERANCE) << "There is a closer point"; 164 | }else{ 165 | EXPECT_EQ(len, M) << "Some intersection should be found"; 166 | } 167 | } 168 | } 169 | 170 | 171 | } 172 | -------------------------------------------------------------------------------- /gdip/core/test/diptest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file circletest.h 3 | * @author Petr Vana 4 | * @brief Test implementation of the DIP 5 | */ 6 | 7 | #include "gtest/gtest.h" 8 | #include "opendubins/dubins.h" 9 | 10 | using namespace std; 11 | using namespace opendubins; 12 | 13 | namespace maxdiptest { 14 | 15 | const int COUNT = 1000; 16 | 17 | int typeCount[200] = {}; 18 | 19 | void check(AngleInterval &interval1, AngleInterval &interval2, Dubins &d1, Dubins &d2) { 20 | 21 | //if (d2.getType() != Unknown) { 22 | EXPECT_TRUE(d1.check()) << "incorrect original maneuver"; 23 | EXPECT_TRUE(d2.check()) << "incorrect newly created maneuver form intervals"; 24 | EXPECT_TRUE(interval1.inIntervalWithTollerance(d2.start.ang, TOLERANCE)) 25 | << "out of first interval"; 26 | EXPECT_TRUE(interval2.inIntervalWithTollerance(d2.end.ang, TOLERANCE)) 27 | << "out of second interval"; 28 | EXPECT_GE(d1.length + TOLERANCE, d2.length) << "DubinsInterval is longer than original maneuver"; 29 | 30 | double len2 = d2.getFirstArc().getLength() + d2.getSecondArc().getLength(); 31 | if (d2.isCCC) { 32 | len2 += d2.getCenterArc().getLength(); 33 | } else { 34 | len2 += d2.getCenter().getLength(); 35 | } 36 | 37 | EXPECT_LE(fabs(d2.length - len2), TOLERANCE) 38 | << "Incorrect length calculation"; 39 | 40 | typeCount[(int) d2.getType()]++; 41 | } 42 | 43 | Dubins checkUsingIntervals(Dubins &d1, double a1, double b1, double a2, double b2) { 44 | double a = myRandom() * a1; 45 | double b = myRandom() * b1; 46 | 47 | AngleInterval interval1 = AngleInterval(d1.start.point, d1.start.getAngle() - a, b + a); 48 | 49 | a = myRandom() * a2; 50 | b = myRandom() * b2; 51 | 52 | AngleInterval interval2 = AngleInterval(d1.end.point, d1.end.getAngle() - a, b + a); 53 | 54 | Dubins d2; 55 | for(int i = 0; i < 1; i++){ 56 | d2 = Dubins(interval1, interval2, d1.radius); 57 | } 58 | check(interval1, interval2, d1, d2); 59 | 60 | return d2; 61 | } 62 | 63 | double getRadius() { 64 | return 0.1 + 3 * myRandom(); 65 | } 66 | 67 | TEST (DIP, ZeroInterval) { 68 | State p1 = State(Point(0, 0), 0); 69 | State p2 = State(Point(0, 0), 0); 70 | 71 | for (int i = 0; i < COUNT; ++i) { 72 | p1.random(100); 73 | p2.random(100); 74 | Dubins d1(p1, p2, getRadius()); 75 | 76 | AngleInterval interval1 = AngleInterval(p1.point, p1.ang, 0); 77 | AngleInterval interval2 = AngleInterval(p2.point, p2.ang, 0); 78 | Dubins d2(interval1, interval2, d1.radius); 79 | 80 | check(interval1, interval2, d1, d2); 81 | } 82 | } 83 | 84 | TEST (DIP, S) { 85 | State p1 = State(Point(0, 0), 0); 86 | for (int i = 0; i < COUNT; ++i) { 87 | p1.random(100); 88 | Dubins d1(p1, false, 0, myRandom() * 10, 0, getRadius()); 89 | Dubins d2 = checkUsingIntervals(d1, 1, 1, 1, 1); 90 | 91 | EXPECT_TRUE(fabs(d1.length - d2.length) < TOLERANCE) << "Different length of a straight line."; 92 | } 93 | } 94 | 95 | TEST (DIP, RS) { 96 | State p1 = State(Point(0, 0), 0); 97 | for (int i = 0; i < COUNT; ++i) { 98 | p1.random(100); 99 | Dubins d1(p1, false, -myRandom() * 7, myRandom() * 10, 0, getRadius()); 100 | checkUsingIntervals(d1, 0, 1, 1, 1); 101 | } 102 | } 103 | 104 | TEST (DIP, LS) { 105 | State p1 = State(Point(0, 0), 0); 106 | for (int i = 0; i < COUNT; ++i) { 107 | p1.random(100); 108 | Dubins d1(p1, false, myRandom() * 7, myRandom() * 10, 0, getRadius()); 109 | checkUsingIntervals(d1, 1, 0, 1, 1); 110 | } 111 | } 112 | 113 | TEST (DIP, SR) { 114 | State p1 = State(Point(0, 0), 0); 115 | for (int i = 0; i < COUNT; ++i) { 116 | p1.random(100); 117 | Dubins d1(p1, false, 0, myRandom() * 10, -myRandom() * 7, getRadius()); 118 | checkUsingIntervals(d1, 1, 1, 1, 0); 119 | } 120 | } 121 | 122 | TEST (DIP, SL) { 123 | State p1 = State(Point(0, 0), 0); 124 | for (int i = 0; i < COUNT; ++i) { 125 | p1.random(100); 126 | Dubins d1(p1, false, 0, myRandom() * 10, myRandom() * 7, getRadius()); 127 | checkUsingIntervals(d1, 1, 1, 0, 1); 128 | } 129 | } 130 | 131 | TEST (DIP, Lp) { 132 | State p1 = State(Point(0, 0), 0); 133 | for (int i = 0; i < COUNT; ++i) { 134 | p1.random(100); 135 | Dubins d1(p1, false, M_PI + myRandom() * M_PI, 0, 0, getRadius()); 136 | checkUsingIntervals(d1, 0.1, 0.1, 0.1, 0.1); 137 | } 138 | } 139 | 140 | TEST (DIP, Rp) { 141 | State p1 = State(Point(0, 0), 0); 142 | for (int i = 0; i < COUNT; ++i) { 143 | p1.random(100); 144 | Dubins d1(p1, false, -M_PI - myRandom() * M_PI, 0, 0, getRadius()); 145 | checkUsingIntervals(d1, 0.1, 0.1, 0.1, 0.1); 146 | } 147 | } 148 | 149 | TEST (DIP, LRp) { 150 | State p1 = State(Point(0, 0), 0); 151 | for (int i = 0; i < COUNT; ++i) { 152 | p1.random(100); 153 | 154 | const double TOL_ANG = 0.05; 155 | double turn1 = myRandom() * M_PI * (1 - TOL_ANG); 156 | double turn2 = -M_PI * (1 + TOL_ANG) - myRandom() * M_PI * (1 - 2 * TOL_ANG); 157 | 158 | Dubins d1(p1, false, turn1, 0, turn2, getRadius()); 159 | checkUsingIntervals(d1, 0.1, 0, 0.1, 0.1); 160 | } 161 | } 162 | 163 | TEST (DIP, RLp) { 164 | State p1 = State(Point(0, 0), 0); 165 | for (int i = 0; i < COUNT; ++i) { 166 | p1.random(100); 167 | 168 | const double TOL_ANG = 0.05; 169 | double turn1 = -myRandom() * M_PI * (1 - TOL_ANG); 170 | double turn2 = +M_PI * (1 + TOL_ANG) + myRandom() * M_PI * (1 - 2 * TOL_ANG); 171 | 172 | Dubins d1(p1, false, turn1, 0, turn2, getRadius()); 173 | checkUsingIntervals(d1, 0, 0.1, 0.1, 0.1); 174 | } 175 | } 176 | 177 | TEST (DIP, LpR) { 178 | State p1 = State(Point(0, 0), 0); 179 | for (int i = 0; i < COUNT; ++i) { 180 | p1.random(100); 181 | 182 | const double TOL_ANG = 0.05; 183 | double turn1 = M_PI * (1 + TOL_ANG) + myRandom() * M_PI * (1 - 2 * TOL_ANG); 184 | double turn2 = -myRandom() * M_PI * (1 - TOL_ANG); 185 | 186 | Dubins d1(p1, false, turn1, 0, turn2, getRadius()); 187 | checkUsingIntervals(d1, 0.1, 0.1, 0.1, 0); 188 | } 189 | } 190 | 191 | TEST (DIP, RpL) { 192 | State p1 = State(Point(0, 0), 0); 193 | for (int i = 0; i < COUNT; ++i) { 194 | p1.random(100); 195 | 196 | const double TOL_ANG = 0.05; 197 | double turn1 = -M_PI * (1 + TOL_ANG) - myRandom() * M_PI * (1 - 2 * TOL_ANG); 198 | double turn2 = myRandom() * M_PI * (1 - TOL_ANG); 199 | 200 | Dubins d1(p1, false, turn1, 0, turn2, getRadius()); 201 | checkUsingIntervals(d1, 0.1, 0.1, 0, 0.1); 202 | } 203 | } 204 | 205 | TEST (DIP, Small) { 206 | State p1 = State(Point(0, 0), 0); 207 | State p2 = State(Point(0, 0), 0); 208 | for (int i = 0; i < COUNT; ++i) { 209 | p1.random(5); 210 | p2.random(5); 211 | Dubins d1(p1, p2, getRadius()); 212 | checkUsingIntervals(d1, 0.1, 0.1, 0.1, 0.1); 213 | } 214 | } 215 | 216 | TEST (DIP, Big) { 217 | State p1 = State(Point(0, 0), 0); 218 | State p2 = State(Point(0, 0), 0); 219 | for (int i = 0; i < COUNT; ++i) { 220 | p1.random(5); 221 | p2.random(5); 222 | Dubins d1(p1, p2, getRadius()); 223 | checkUsingIntervals(d1, 1, 1, 1, 1); 224 | } 225 | } 226 | 227 | TEST (DIP, Random) { 228 | State p1 = State(Point(0, 0), 0); 229 | for (int i = 0; i < COUNT; ++i) { 230 | p1.random(5); 231 | Dubins d1; 232 | 233 | double radius = getRadius(); 234 | 235 | double turn1 = 0; 236 | double turn2 = 0; 237 | 238 | if (myRandom() < 0.5) { 239 | double center = myRandom() * radius; 240 | d1 = Dubins(p1, false, turn1, center, turn2, radius); 241 | } else { 242 | double centerTurn = (1 + myRandom()) * M_PI; 243 | d1 = Dubins(p1, true, turn1, centerTurn, turn2, radius); 244 | } 245 | checkUsingIntervals(d1, 0.1, 0.1, 0.1, 0.1); 246 | } 247 | } 248 | 249 | TEST (DIP, RpLp) { 250 | 251 | // int localType[200] = {}; 252 | 253 | State p1 = State(Point(0, 0), 0); 254 | for (int i = 0; i < COUNT; ++i) { 255 | p1.random(100); 256 | double turn = M_PI + myRandom() * M_PI; 257 | Dubins d1(p1, false, -turn, TOLERANCE, turn, getRadius()); 258 | auto epsilon = 0.00001 * myRandom(); 259 | auto d2 = checkUsingIntervals(d1, epsilon, epsilon, epsilon, epsilon); 260 | } 261 | } 262 | 263 | TEST (DIP, LpRp) { 264 | State p1 = State(Point(0, 0), 0); 265 | for (int i = 0; i < COUNT; ++i) { 266 | p1.random(100); 267 | double turn = M_PI + myRandom() * M_PI; 268 | Dubins d1(p1, false, turn, 0, -turn, getRadius()); 269 | // double intervalSize = 3; 270 | checkUsingIntervals(d1, 0.1, 0.1, 0.1, 0.1); 271 | } 272 | } 273 | 274 | // this test proves that there is no locally optimal XpXp maneuver 275 | TEST (DIP, RpLp_epsilon) { 276 | 277 | State p1 = State(Point(0, 0), 0); 278 | for (int i = 0; i < COUNT*10; ++i) { 279 | p1.random(100); 280 | double turn = M_PI + (0.11 * myRandom()) * M_PI; 281 | Dubins d1(p1, false, -turn, 0, turn, getRadius()); 282 | auto epsilon = 0.01 * myRandom(); 283 | 284 | auto p1a = d1.start; 285 | p1a.ang += epsilon; 286 | auto p1b = d1.start; 287 | p1b.ang -= epsilon; 288 | 289 | auto p2a = d1.end; 290 | p2a.ang += epsilon; 291 | auto p2b = d1.end; 292 | p2b.ang -= epsilon; 293 | 294 | auto da = Dubins(p1a, p2a, d1.radius); 295 | auto db = Dubins(p1b, p2a, d1.radius); 296 | auto dc = Dubins(p1a, p2b, d1.radius); 297 | auto dd = Dubins(p1b, p2b, d1.radius); 298 | 299 | auto nLen = min(min(da.length, db.length), min(dc.length, dd.length)); 300 | 301 | /*cout << "da " << da.getType() << endl << da << endl; 302 | cout << "db " << db.getType() << endl << db << endl; 303 | cout << "dc " << dc.getType() << endl << dc << endl; 304 | cout << "dd " << dd.getType() << endl << dd << endl;*/ 305 | 306 | //cout << endl << "diff " << (d1.length - nLen) << endl; 307 | 308 | EXPECT_GE(d1.length, nLen - 0.00000001) << " Locally optimal "; 309 | } 310 | } 311 | 312 | TEST (DIP, AllUsed) { 313 | 314 | const int MIN = (int) DType::DIP_S; 315 | #if USE_XpXp 316 | const int MAX = (int) DType::DIP_RpLp; 317 | #else 318 | const int MAX = (int) DType::DIP_RLR; 319 | #endif 320 | 321 | for (int t = MIN; t <= MAX; t++) { 322 | DType typ = static_cast(t); 323 | const int value = typeCount[t]; 324 | std::cout << typ << "\t-\t" << value << endl; 325 | EXPECT_GT(value, 0) << "There is no maneuver with type " << typ; 326 | } 327 | } 328 | 329 | } 330 | -------------------------------------------------------------------------------- /gdip/core/test/dubinstest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file circletest.h 3 | * @author Petr Vana 4 | * @brief Test implementation of the Dubins maneuver 5 | */ 6 | 7 | #include "gtest/gtest.h" 8 | #include "opendubins/dubins.h" 9 | 10 | using namespace opendubins; 11 | 12 | namespace dubinstest { 13 | 14 | const int COUNT = 100000; 15 | const int B_COUNT = 1; 16 | 17 | TEST (DubinsTest, CorrectPosition) { 18 | State p1 = State(0, 0, 0); 19 | State p2 = State(0, 0, 0); 20 | 21 | Dubins d(p1, p2, 1); 22 | 23 | for (int i = 0; i < COUNT; ++i) { 24 | p1.random(10); 25 | p2.random(10); 26 | for (int j = 0; j < B_COUNT; ++j) { 27 | d = Dubins(p1, p2, 1); 28 | } 29 | 30 | EXPECT_TRUE(d.check()); 31 | } 32 | } 33 | 34 | TEST (DubinsTest, ShortestCSC) { 35 | State p1 = State(0, 0, 0); 36 | State p2 = State(0, 0, 0); 37 | Dubins d1(p1, p2, 100); 38 | Dubins d2(p1, p2, 100); 39 | 40 | for (int i = 0; i < COUNT; ++i) { 41 | p1.random(100); 42 | d1 = Dubins(p1, false, myRandom() * 20 - 10, myRandom() * 10, myRandom() * 20 - 10, 1.0); 43 | for (int j = 0; j < B_COUNT; ++j) { 44 | d2 = Dubins(p1, d1.end, d1.radius); 45 | } 46 | 47 | EXPECT_TRUE(d1.check()); 48 | EXPECT_TRUE(d2.check()); 49 | EXPECT_GE(d1.length + 10 * TOLERANCE, d2.length); 50 | 51 | double turnSum = 0; 52 | if(d2.getType() == DType::LSL || d2.getType() == DType::RSR) { 53 | turnSum += std::fabs(d2.getLen1()); 54 | turnSum += std::fabs(d2.getLen3()); 55 | EXPECT_LE(turnSum, 2 * M_PI) << "Sum of turn segments si longer than 2 PI in type " << d2.getType(); 56 | } 57 | 58 | if(d2.getType() == DType::LRL || d2.getType() == DType::RLR){ 59 | EXPECT_GT(std::fabs(d2.getLen2()), M_PI) << "Center segment should be longer than PI " << d2; 60 | } 61 | } 62 | } 63 | 64 | // TODO - add test for CCC maneuver 65 | 66 | } -------------------------------------------------------------------------------- /gdip/core/test/gdiptest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file circletest.h 3 | * @author Petr Vana 4 | * @brief Test of the optimal solution for the Generalized Dubins Interval Problem (GDIP) 5 | */ 6 | 7 | #include "gtest/gtest.h" 8 | #include "opendubins/dubins.h" 9 | 10 | using namespace opendubins; 11 | using namespace std; 12 | 13 | namespace gdiptest { 14 | 15 | const int COUNT = 2000; 16 | 17 | int typeCount[200] = {}; 18 | 19 | void check2(AngleInterval &interval1, AngleInterval &interval2, Dubins &d1, Dubins &d2) { 20 | EXPECT_TRUE(d1.check()) << "incorrect original maneuver"; 21 | EXPECT_TRUE(d2.check()) << "incorrect newly created maneuver form intervals " << endl << d2; 22 | if(!d2.check()){ 23 | cout << "Debug point here" << endl; 24 | } 25 | 26 | bool in1 = interval1.inIntervalWithTollerance(d2.start.ang, TOLERANCE); 27 | bool in2 = interval2.inIntervalWithTollerance(d2.end.ang, TOLERANCE); 28 | EXPECT_TRUE(in1) << "out of first interval"; 29 | EXPECT_TRUE(in2) << "out of second interval"; 30 | EXPECT_GE(d1.length + 10 * TOLERANCE, d2.length) << "DubinsInterval " << d2.getType() << " is longer than original maneuver " << d1.getType(); 31 | 32 | if (!in1) { 33 | cout << "IN1 " << d2.start.ang << " is not in <" << interval1.getRight() << ", " << interval1.getLeft() << 34 | endl; 35 | } 36 | if (!in2) { 37 | cout << "IN2 " << d2.end.ang << " is not in <" << interval2.getRight() << ", " << interval2.getLeft() << 38 | endl; 39 | } 40 | 41 | if (d1.length + TOLERANCE < d2.length) { 42 | std::cout << "First dubins 111111111111 " << std::endl << d1 << std::endl; 43 | std::cout << "Second dubins 222222222222 " << std::endl << d2 << std::endl; 44 | } 45 | 46 | typeCount[(int) d2.getType()]++; 47 | } 48 | 49 | bool checkGDIP(AngleInterval &interval1, AngleInterval &interval2, Dubins &d1, Dubins &d2, double diff2) { 50 | bool ret = true; 51 | 52 | EXPECT_TRUE(d1.check()) << "incorrect original maneuver"; 53 | EXPECT_TRUE(d2.check()) << "incorrect newly created maneuver form intervals"; 54 | 55 | bool in1 = interval1.inIntervalWithTollerance(d2.start.ang, TOLERANCE); 56 | bool in2 = interval2.inIntervalWithTollerance(d2.end.ang, TOLERANCE); 57 | EXPECT_TRUE(in1) << "out of first interval"; 58 | EXPECT_TRUE(in2) << "out of second interval"; 59 | EXPECT_GE(d1.length + 1 * TOLERANCE, d2.length) << "DubinsInterval is longer than original maneuver"; 60 | 61 | if (!in1) { 62 | cout << "IN1 " << d2.start.ang << " is not in <" << interval1.getRight() << ", " << interval1.getLeft() << 63 | endl; 64 | } 65 | if (!in2) { 66 | cout << "IN2 " << d2.end.ang << " is not in <" << interval2.getRight() << ", " << interval2.getLeft() << 67 | endl; 68 | } 69 | 70 | if (d1.length + TOLERANCE < d2.length) { 71 | std::cout << "First dubins 111111111111 " << std::endl << d1 << std::endl; 72 | std::cout << "Second dubins 222222222222 " << std::endl << d2 << std::endl; 73 | ret = false; 74 | } 75 | 76 | EXPECT_GE(TOLERANCE, interval1.point.distance(d2.start.point)) << "Wrong start " << interval1.point << endl << d2; 77 | EXPECT_GE(diff2 + TOLERANCE, interval2.point.distance(d2.end.point)) << "Wrong end " << interval2.point << endl << d2; 78 | 79 | typeCount[(int) d2.getType()]++; 80 | 81 | return ret; 82 | } 83 | 84 | Dubins checkUsingIntervals2(Dubins &d1, double a1, double b1, double a2, double b2) { 85 | double a = myRandom() * a1; 86 | double b = myRandom() * b1; 87 | 88 | AngleInterval interval1 = AngleInterval(d1.start.point, d1.start.getAngle() - a, b + a); 89 | 90 | a = myRandom() * a2; 91 | b = myRandom() * b2; 92 | 93 | AngleInterval interval2 = AngleInterval(d1.end.point, d1.end.getAngle() - a, b + a); 94 | 95 | Dubins d2(interval1, interval2, d1.radius, 0, 0); 96 | check2(interval1, interval2, d1, d2); 97 | 98 | return d2; 99 | } 100 | 101 | double getRadius2() { 102 | return 0.1 + 3 * myRandom(); 103 | } 104 | 105 | // OLD TESTS FROM DIP /////////////////////////////////////////////////////////////////// 106 | 107 | TEST (GDIP, ZeroInterval) { 108 | State p1 = State(Point(0, 0), 0); 109 | State p2 = State(Point(0, 0), 0); 110 | 111 | for (int i = 0; i < COUNT; ++i) { 112 | p1.random(100); 113 | p2.random(100); 114 | Dubins d1(p1, p2, getRadius2()); 115 | 116 | AngleInterval interval1 = AngleInterval(p1.point, p1.ang, 0); 117 | AngleInterval interval2 = AngleInterval(p2.point, p2.ang, 0); 118 | Dubins d2(interval1, interval2, d1.radius, 0, 0); 119 | 120 | check2(interval1, interval2, d1, d2); 121 | 122 | Dubins d3(interval1, interval2, d1.radius, 0, 0); 123 | } 124 | } 125 | 126 | TEST (GDIP, S) { 127 | State p1 = State(Point(0, 0), 0); 128 | for ( 129 | int i = 0; 130 | i < COUNT; 131 | ++i) { 132 | p1.random(100); 133 | Dubins d1(p1, false, 0, myRandom() * 10, 0, getRadius2()); 134 | Dubins d2 = checkUsingIntervals2(d1, 1, 1, 1, 1); 135 | 136 | EXPECT_LE(fabs(d1.length - d2.length), TOLERANCE) << "Different length of a straight line." 137 | << endl << d1 << endl << d2; 138 | } 139 | } 140 | 141 | TEST (GDIP, RS) { 142 | State p1 = State(Point(0, 0), 0); 143 | for ( 144 | int i = 0; 145 | i < COUNT; 146 | ++i) { 147 | p1.random(100); 148 | Dubins d1(p1, false, -myRandom() * 7, myRandom() * 10, 0, getRadius2()); 149 | checkUsingIntervals2(d1, 0, 1, 1, 1); 150 | } 151 | } 152 | 153 | TEST (GDIP, LS) { 154 | State p1 = State(Point(0, 0), 0); 155 | for ( 156 | int i = 0; 157 | i < COUNT; 158 | ++i) { 159 | p1.random(100); 160 | Dubins d1(p1, false, myRandom() * 7, myRandom() * 10, 0, getRadius2()); 161 | checkUsingIntervals2(d1, 1, 0, 1, 1); 162 | } 163 | } 164 | 165 | TEST (GDIP, SR) { 166 | State p1 = State(Point(0, 0), 0); 167 | for ( 168 | int i = 0; 169 | i < COUNT; 170 | ++i) { 171 | p1.random(100); 172 | Dubins d1(p1, false, 0, myRandom() * 10, -myRandom() * 7, getRadius2()); 173 | checkUsingIntervals2(d1, 1, 1, 1, 0); 174 | } 175 | } 176 | 177 | TEST (GDIP, SL) { 178 | State p1 = State(Point(0, 0), 0); 179 | for ( 180 | int i = 0; 181 | i < COUNT; 182 | ++i) { 183 | p1.random(100); 184 | Dubins d1(p1, false, 0, myRandom() * 10, myRandom() * 7, getRadius2()); 185 | checkUsingIntervals2(d1, 1, 1, 0, 1); 186 | } 187 | } 188 | 189 | TEST (GDIP, Lp) { 190 | State p1 = State(Point(0, 0), 0); 191 | for ( 192 | int i = 0; 193 | i < COUNT; 194 | ++i) { 195 | p1.random(100); 196 | Dubins d1(p1, false, M_PI + myRandom() * M_PI, 0, 0, getRadius2()); 197 | checkUsingIntervals2(d1, 0.1, 0.1, 0.1, 0.1); 198 | } 199 | } 200 | 201 | TEST (GDIP, Rp) { 202 | State p1 = State(Point(0, 0), 0); 203 | for (int i = 0; i < COUNT; ++i) { 204 | p1.random(100); 205 | Dubins d1(p1, false, -M_PI - myRandom() * M_PI, 0, 0, getRadius2()); 206 | checkUsingIntervals2(d1, 0.1, 0.1, 0.1, 0.1); 207 | } 208 | } 209 | 210 | TEST (GDIP, LRp) { 211 | State p1 = State(Point(0, 0), 0); 212 | for (int i = 0; i < COUNT; ++i) { 213 | p1.random(100); 214 | 215 | const double TOL_ANG = 0.05; 216 | double turn1 = myRandom() * M_PI * (1 - TOL_ANG); 217 | double turn2 = -M_PI * (1 + TOL_ANG) - myRandom() * M_PI * (1 - 2 * TOL_ANG); 218 | 219 | Dubins d1(p1, false, turn1, 0, turn2, getRadius2()); 220 | checkUsingIntervals2(d1, 0.1, 0, 0.1, 0.1); 221 | } 222 | } 223 | 224 | TEST (GDIP, RLp) { 225 | State p1 = State(Point(0, 0), 0); 226 | for ( 227 | int i = 0; 228 | i < COUNT; 229 | ++i) { 230 | p1.random(100); 231 | 232 | const double TOL_ANG = 0.05; 233 | double turn1 = -myRandom() * M_PI * (1 - TOL_ANG); 234 | double turn2 = +M_PI * (1 + TOL_ANG) + myRandom() * M_PI * (1 - 2 * TOL_ANG); 235 | 236 | Dubins d1(p1, false, turn1, 0, turn2, getRadius2()); 237 | checkUsingIntervals2(d1, 0, 0.1, 0.1, 0.1); 238 | } 239 | } 240 | 241 | TEST (GDIP, LpR) { 242 | State p1 = State(Point(0, 0), 0); 243 | for ( 244 | int i = 0; 245 | i < COUNT; 246 | ++i) { 247 | p1.random(100); 248 | 249 | const double TOL_ANG = 0.05; 250 | double turn1 = M_PI * (1 + TOL_ANG) + myRandom() * M_PI * (1 - 2 * TOL_ANG); 251 | double turn2 = -myRandom() * M_PI * (1 - TOL_ANG); 252 | 253 | Dubins d1(p1, false, turn1, 0, turn2, getRadius2()); 254 | checkUsingIntervals2(d1, 0.1, 0.1, 0.1, 0); 255 | } 256 | } 257 | 258 | TEST (GDIP, RpL) { 259 | State p1 = State(Point(0, 0), 0); 260 | for ( 261 | int i = 0; 262 | i < COUNT; 263 | ++i) { 264 | p1.random(100); 265 | 266 | const double TOL_ANG = 0.05; 267 | double turn1 = -M_PI * (1 + TOL_ANG) - myRandom() * M_PI * (1 - 2 * TOL_ANG); 268 | double turn2 = myRandom() * M_PI * (1 - TOL_ANG); 269 | 270 | Dubins d1(p1, false, turn1, 0, turn2, getRadius2()); 271 | checkUsingIntervals2(d1, 0.1, 0.1, 0, 0.1); 272 | } 273 | } 274 | 275 | TEST (GDIP, Small) { 276 | State p1 = State(Point(0, 0), 0); 277 | State p2 = State(Point(0, 0), 0); 278 | for ( 279 | int i = 0; 280 | i < COUNT; 281 | ++i) { 282 | p1.random(5); 283 | p2.random(5); 284 | Dubins d1(p1, p2, getRadius2()); 285 | checkUsingIntervals2(d1, 0.1, 0.1, 0.1, 0.1); 286 | } 287 | } 288 | 289 | TEST (GDIP, Big) { 290 | State p1 = State(Point(0, 0), 0); 291 | State p2 = State(Point(0, 0), 0); 292 | for ( 293 | int i = 0; 294 | i < COUNT; 295 | ++i) { 296 | p1.random(5); 297 | p2.random(5); 298 | Dubins d1(p1, p2, getRadius2()); 299 | checkUsingIntervals2(d1, 1, 1, 1, 1); 300 | } 301 | } 302 | 303 | TEST (GDIP, Random) { 304 | State p1 = State(Point(0, 0), 0); 305 | for ( 306 | int i = 0; 307 | i < COUNT; 308 | ++i) { 309 | p1.random(5); 310 | Dubins d1; 311 | 312 | double radius = getRadius2(); 313 | 314 | double turn1 = 0; 315 | double turn2 = 0; 316 | 317 | if ( myRandom() < 0.5) { 318 | double center = myRandom() * radius; 319 | d1 = Dubins(p1, false, turn1, center, turn2, radius); 320 | } else { 321 | double centerTurn = (1 + myRandom()) * M_PI; 322 | d1 = Dubins(p1, true, turn1, centerTurn, turn2, radius); 323 | } 324 | checkUsingIntervals2(d1, 0.1, 0.1, 0.1, 0.1); 325 | } 326 | } 327 | 328 | ///////////////////////////////////////////////////////////////////////////////// 329 | ///////////////////////////////////////////////////////////////////////////////// 330 | ///////////////////////////////////////////////////////////////////////////////// 331 | ///////////////////////////////////////////////////////////////////////////////// 332 | // NEW TESTS SPECIALLY FOR GDIP ///////////////////////////////////////////////// 333 | ///////////////////////////////////////////////////////////////////////////////// 334 | ///////////////////////////////////////////////////////////////////////////////// 335 | ///////////////////////////////////////////////////////////////////////////////// 336 | ///////////////////////////////////////////////////////////////////////////////// 337 | 338 | 339 | Dubins checkUsingIntervals_GDIP(Dubins &d1, double a1, double b1, double a2, double b2, bool random = false, double diff2 = 0.1) { 340 | diff2 *= myRandom(); 341 | 342 | auto dir2 = Vector(myRandom() * 2 * M_PI) * diff2 * (1 - TOLERANCE); 343 | if(random){ 344 | dir2 *= myRandom(); 345 | } 346 | 347 | double a = myRandom() * a1; 348 | double b = myRandom() * b1; 349 | 350 | AngleInterval interval1 = AngleInterval(d1.start.point, d1.start.getAngle() - a, b + a); 351 | 352 | a = myRandom() * a2; 353 | b = myRandom() * b2; 354 | 355 | AngleInterval interval2 = AngleInterval(d1.end.point + dir2, d1.end.getAngle() - a, b + a); 356 | 357 | Dubins d2(interval1, interval2, d1.radius, diff2); 358 | bool ok = checkGDIP(interval1, interval2, d1, d2, diff2); 359 | 360 | if(!ok){ 361 | cout << "Test is not ok " << endl << "( " << a1 << " " << a2 << " " << b1 << " " << b2 << " " << diff2 << " )" << endl; 362 | } 363 | 364 | return d2; 365 | } 366 | 367 | TEST (GDIP, S_GDIP) { 368 | State p1 = State(Point(0, 0), 0); 369 | for (int i = 0; i < COUNT; ++i) { 370 | p1.random(100); 371 | Dubins d1(p1, false, 0, myRandom() * 10, 0, getRadius2()); 372 | checkUsingIntervals_GDIP(d1, 1, 1, 1, 1); 373 | } 374 | } 375 | 376 | void testCpC(bool isLeft, bool basic, bool reverse = false) { 377 | int sgn = isLeft ? 1 : -1; 378 | 379 | State p1 = State(Point(0, 0), 0); 380 | for (int i = 0; i < COUNT; ++i) { 381 | p1.random(100); 382 | 383 | const double TOL_ANG = 0.05; 384 | double turn1 = myRandom() * M_PI * (1 - TOL_ANG); 385 | double turn2 = -M_PI * (1 + TOL_ANG) - myRandom() * M_PI * (1 - 2 * TOL_ANG); 386 | 387 | if(reverse){ 388 | std::swap(turn1, turn2); 389 | sgn = -1; 390 | } 391 | 392 | Dubins d1(p1, false, sgn*turn1, 0, sgn*turn2, getRadius2()); 393 | 394 | double D = 0.1; 395 | checkUsingIntervals_GDIP(d1, 0, D, D, D, true); 396 | checkUsingIntervals_GDIP(d1, D, 0, D, D, true); 397 | checkUsingIntervals_GDIP(d1, D, D, 0, D, true); 398 | checkUsingIntervals_GDIP(d1, D, D, D, 0, true); 399 | 400 | } 401 | } 402 | 403 | #define TEST_CpC(name, isLeft, reverse) \ 404 | TEST (GDIP, name) { \ 405 | testCpC(isLeft, false, reverse); \ 406 | } 407 | 408 | 409 | TEST_CpC(LpP_GDIP, true, true); 410 | TEST_CpC(RpR_GDIP, false, true); 411 | TEST_CpC(LRp_GDIP, true, false); 412 | TEST_CpC(RLp_GDIP, false, false); 413 | 414 | void testCS(bool isLeft, bool basic, bool straight = false, bool longer = false) { 415 | int sgn = isLeft ? 1 : -1; 416 | 417 | double s = straight ? myRandom() * 0.01 : 0; 418 | 419 | State p1 = State(Point(0, 0), 0); 420 | for (int i = 0; i < COUNT; ++i) { 421 | p1.random(100); 422 | Dubins d1(p1, false, sgn * myRandom() * 3, s, 0, getRadius2()); 423 | if(basic) { 424 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0); 425 | } else { 426 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0, true); 427 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0, true); 428 | } 429 | } 430 | } 431 | 432 | #define TEST_C(name, isLeft) \ 433 | TEST (GDIP, name) { \ 434 | testCS(isLeft, false); \ 435 | } 436 | 437 | #define TEST_C_BASIC(name, isLeft) \ 438 | TEST (GDIP, name) { \ 439 | testCS(isLeft, true); \ 440 | } 441 | 442 | #define TEST_CS(name, isLeft, straight, longer) \ 443 | TEST (GDIP, name) { \ 444 | testCS(isLeft, false, straight, longer); \ 445 | } 446 | 447 | #define TEST_CS_BASIC(name, isLeft, straight, longer) \ 448 | TEST (GDIP, name) { \ 449 | testCS(isLeft, true, straight, longer); \ 450 | } 451 | 452 | 453 | TEST_C(L_GDIP_Random, true); 454 | TEST_C(R_GDIP_Random, false); 455 | 456 | TEST_C_BASIC(L_GDIP_Basic, true); 457 | TEST_C_BASIC(R_GDIP_Basic, false); 458 | 459 | TEST_CS(LS_GDIP_Random, true, true, false); 460 | TEST_CS(RS_GDIP_Random, false, true, false); 461 | 462 | TEST_CS_BASIC(LS_GDIP_Basic, true, true, false); 463 | TEST_CS_BASIC(RS_GDIP_Basic, false, true, false); 464 | 465 | TEST_CS(LS_GDIP_Long, true, true, true); 466 | TEST_CS(RS_GDIP_Long, false, true, true); 467 | 468 | TEST_CS_BASIC(LS_GDIP_Basic_Long, true, true, true); 469 | TEST_CS_BASIC(RS_GDIP_Basic_Long, false, true, true); 470 | 471 | void testSC(bool isLeft, bool basic, bool straight = false, bool longer = false) { 472 | int sgn = isLeft ? 1 : -1; 473 | 474 | double s = straight ? myRandom() * 0.01 : 0; 475 | 476 | State p1 = State(Point(0, 0), 0); 477 | for (int i = 0; i < COUNT; ++i) { 478 | p1.random(100); 479 | Dubins d1(p1, false, 0, s, sgn * myRandom() * 3, getRadius2()); 480 | if(basic) { 481 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0); 482 | } else { 483 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0, true); 484 | } 485 | } 486 | } 487 | 488 | #define TEST_CB(name, isLeft) \ 489 | TEST (GDIP, name) { \ 490 | testSC(isLeft, false); \ 491 | } 492 | 493 | #define TEST_CB_BASIC(name, isLeft) \ 494 | TEST (GDIP, name) { \ 495 | testSC(isLeft, true); \ 496 | } 497 | 498 | #define TEST_SC(name, isLeft, straight, longer) \ 499 | TEST (GDIP, name) { \ 500 | testSC(isLeft, false, straight, longer); \ 501 | } 502 | 503 | #define TEST_SC_BASIC(name, isLeft, straight, longer) \ 504 | TEST (GDIP, name) { \ 505 | testSC(isLeft, true, straight, longer); \ 506 | } 507 | 508 | 509 | TEST_CB(LB_GDIP_Random, true); 510 | TEST_CB(RB_GDIP_Random, false); 511 | 512 | TEST_CB_BASIC(LB_GDIP_Basic, true); 513 | TEST_CB_BASIC(RB_GDIP_Basic, false); 514 | 515 | TEST_SC(SL_GDIP_Random, true, true, false); 516 | TEST_SC(SR_GDIP_Random, false, true, false); 517 | 518 | TEST_SC_BASIC(SL_GDIP_Basic, true, true, false); 519 | TEST_SC_BASIC(SR_GDIP_Basic, false, true, false); 520 | 521 | TEST_SC(SL_GDIP_Long, true, true, true); 522 | TEST_SC(SR_GDIP_Long, false, true, true); 523 | 524 | TEST_SC_BASIC(SL_GDIP_Basic_Long, true, true, true); 525 | TEST_SC_BASIC(SR_GDIP_Basic_Long, false, true, true); 526 | 527 | 528 | void testCSC(bool isLeft, bool basic, bool same, double straight = 0, int type = 0) { 529 | int sgn1 = isLeft ? 1 : -1; 530 | int sgn2 = (isLeft==same) ? 1 : -1; 531 | 532 | double s = myRandom() * straight; 533 | 534 | State p1 = State(Point(0, 0), 0); 535 | for (int i = 0; i < COUNT*1; ++i) { 536 | p1.random(100); 537 | double r1, r2; 538 | 539 | if(type == 1){ 540 | r1 = sgn1 * myRandom() * 1; 541 | r2 = fabs(myRandom() * r1) * sgn2; 542 | }else if (type == 2){ 543 | r2 = sgn2 * myRandom() * 1; 544 | r1 = fabs(myRandom() * r2) * sgn1; 545 | } else{ 546 | r1 = sgn1 * myRandom() * 1; 547 | r2 = sgn2 * myRandom() * 1; 548 | } 549 | 550 | Dubins d1(p1, false, r1, s, r2, getRadius2()); 551 | if(basic) { 552 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0); 553 | } else { 554 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0, true, 0.01); 555 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0, true, 0.1); 556 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0, true, 1); 557 | checkUsingIntervals_GDIP(d1, 0, 0, 0, 0, true, 3); 558 | } 559 | } 560 | } 561 | 562 | #define TEST_CC(name, isLeft, same) \ 563 | TEST (GDIP, name) { \ 564 | testCSC(isLeft, false, same); \ 565 | } 566 | 567 | #define TEST_CC_BASIC(name, isLeft, same) \ 568 | TEST (GDIP, name) { \ 569 | testCSC(isLeft, true, same); \ 570 | } 571 | 572 | #define TEST_CSC_ALL(name, isLeft, false, same, straight) \ 573 | TEST (GDIP, name) { \ 574 | testCSC(isLeft, false, same, straight, 0); \ 575 | }\ 576 | TEST (GDIP, name##_Xx) { \ 577 | testCSC(isLeft, false, same, straight, 1); \ 578 | }\ 579 | TEST (GDIP, name##_xX) { \ 580 | testCSC(isLeft, false, same, straight, 2); \ 581 | } 582 | 583 | #define TEST_CSC(name, isLeft, same, straight) \ 584 | TEST_CSC_ALL(name, isLeft, false, same, straight) 585 | 586 | #define TEST_CSC_BASIC(name, isLeft, same, straight) \ 587 | TEST_CSC_ALL(name, isLeft, true, same, straight) 588 | 589 | #define BOOL_L true 590 | #define BOOL_R false 591 | 592 | #define TEST_CSC_BATCH(first, second) \ 593 | TEST_CC_BASIC(first##second##_GDIP_Basic, BOOL_##first, BOOL_##second) \ 594 | TEST_CC(first##second##_GDIP_Random, BOOL_##first, BOOL_##second) \ 595 | TEST_CSC_BASIC(first##S##second##_GDIP_Basic, BOOL_##first, BOOL_##second, 1) \ 596 | TEST_CSC(first##S##second##_GDIP_Random, BOOL_##first, BOOL_##second, 1) 597 | 598 | // SAME SIDE 599 | TEST_CSC_BATCH(L, L); 600 | TEST_CSC_BATCH(R, R); 601 | 602 | // DIFF SIDE 603 | TEST_CSC_BATCH(R, L); 604 | TEST_CSC_BATCH(L, R); 605 | 606 | 607 | TEST (GDIP, AllUsed) { 608 | for (int t = (int)DType::GDIP_NO; t <= (int)DType::GDIP_RpL; t++) { 609 | DType typ = static_cast(t); 610 | const int value = typeCount[t]; 611 | std::cout << typ << "\t-\t" << value << endl; 612 | EXPECT_GT(value, 0) << "There is no maneuver with type " << typ; 613 | } 614 | } 615 | 616 | } -------------------------------------------------------------------------------- /gdip/core/test/speedtest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file circletest.h 3 | * @author Petr Vana 4 | * @brief Compare speed of original Dubins maneuver, DIP, and GDIP implementation 5 | */ 6 | 7 | #include "gtest/gtest.h" 8 | #include "opendubins/dubins.h" 9 | 10 | using namespace opendubins; 11 | using namespace std; 12 | 13 | namespace speedtest { 14 | 15 | const int COUNT = 1000; 16 | const int COUNT2 = 1000; 17 | 18 | double box = 10; 19 | 20 | TEST (SPEED, GDIP) { 21 | double sum = 0; 22 | for(int i = 0; i < COUNT; i++) { 23 | double radius = 0; 24 | 25 | double a1 = 2 * M_PI * myRandom(); 26 | double b1 = 2 * M_PI * myRandom(); 27 | double a2 = 2 * M_PI * myRandom(); 28 | double b2 = 2 * M_PI * myRandom(); 29 | 30 | Point p1(box * myRandom(), box * myRandom()); 31 | Point p2(box * myRandom(), box * myRandom()); 32 | 33 | AngleInterval interval1 = AngleInterval(p1, -a1, b1 + a1); 34 | AngleInterval interval2 = AngleInterval(p2, -a2, b2 + a2); 35 | 36 | double diff1 = myRandom(); 37 | double diff2 = myRandom(); 38 | 39 | for(int j = 0; j < COUNT2; j++) { 40 | Dubins d2(interval1, interval2, radius, diff1, diff2); 41 | sum += d2.length; 42 | } 43 | } 44 | if(sum == 0) std::cout << "Error" << std::endl; 45 | } 46 | 47 | TEST (SPEED, DIP) { 48 | double sum = 0; 49 | for(int i = 0; i < COUNT; i++) { 50 | double radius = 0; 51 | 52 | double a1 = 2 * M_PI * myRandom(); 53 | double b1 = 2 * M_PI * myRandom(); 54 | double a2 = 2 * M_PI * myRandom(); 55 | double b2 = 2 * M_PI * myRandom(); 56 | 57 | Point p1(box * myRandom(), box * myRandom()); 58 | Point p2(box * myRandom(), box * myRandom()); 59 | 60 | AngleInterval interval1 = AngleInterval(p1, -a1, b1 + a1); 61 | AngleInterval interval2 = AngleInterval(p2, -a2, b2 + a2); 62 | 63 | for(int j = 0; j < COUNT2; j++) { 64 | Dubins d2(interval1, interval2, radius); 65 | sum += d2.length; 66 | } 67 | } 68 | if(sum == 0) std::cout << "Error" << std::endl; 69 | } 70 | 71 | TEST (SPEED, Dubins) { 72 | double sum = 0; 73 | for(int i = 0; i < COUNT; i++) { 74 | double radius = 0; 75 | 76 | double a1 = 2 * M_PI * myRandom(); 77 | double b1 = 2 * M_PI * myRandom(); 78 | double a2 = 2 * M_PI * myRandom(); 79 | double b2 = 2 * M_PI * myRandom(); 80 | 81 | Point p1(box * myRandom(), box * myRandom()); 82 | Point p2(box * myRandom(), box * myRandom()); 83 | 84 | AngleInterval interval1 = AngleInterval(p1, -a1, b1 + a1); 85 | AngleInterval interval2 = AngleInterval(p2, -a2, b2 + a2); 86 | 87 | for(int j = 0; j < COUNT2; j++) { 88 | Dubins d2(interval1.getLeftState(), interval2.getLeftState(), radius); 89 | sum += d2.length; 90 | } 91 | } 92 | if(sum == 0) std::cout << "Error" << std::endl; 93 | } 94 | 95 | } -------------------------------------------------------------------------------- /gdip/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories("../core/src") 2 | 3 | add_executable(hellodubins src/hellodubins.cpp ${SOURCE_FILES} ) 4 | target_link_libraries(hellodubins opendubins_core) 5 | 6 | install(TARGETS hellodubins DESTINATION bin) 7 | 8 | add_executable(gdipexample src/gdipexample.cpp ${SOURCE_FILES} ) 9 | target_link_libraries(gdipexample opendubins_core) 10 | 11 | install(TARGETS gdipexample DESTINATION bin) -------------------------------------------------------------------------------- /gdip/examples/src/gdipexample.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "opendubins/dubins.h" 4 | 5 | using namespace std; 6 | using namespace opendubins; 7 | 8 | int main() { 9 | 10 | cout << "Hello, this is example of optimal solution of the GDIP!" << endl; 11 | 12 | // Centers of two given regions 13 | Point p1(0,0); 14 | Point p2(5,5); 15 | 16 | // Radii of the regions 17 | double region1radius = 0.5; 18 | double region2radius = 2; 19 | 20 | cout << "Let have two locations: " << endl << p1 << "," << endl << p2 << "." << endl; 21 | 22 | cout << "Now, we define intervals of the corresponding heading angles." << endl; 23 | /* AngleInterval has three parameters: 24 | * 1) Position of the center 25 | * 2) Right limit of the angle interval [ 2) = minimum angle ] 26 | * 3) Interval size [ 2) + 3) = maximum angle ] 27 | */ 28 | AngleInterval a1(p1, 0, M_PI/2); 29 | AngleInterval a2(p2, M_PI, M_PI/2); 30 | /* Thus, interval of a1 is [0, M_PI/2] 31 | * and nterval of a2 is [M_PI, 3*M_PI/2]. 32 | */ 33 | 34 | cout << "The angle intervals are: " << endl << a1 << "," << endl << a2 << "." << endl; 35 | cout << "The goal is to find the shortest Dubins path between two regions." 36 | << "The first region has radius " << region1radius << ", and the second one " << region2radius << "." << endl; 37 | 38 | // Minimum turning radius 39 | double radius = 1.0; 40 | 41 | Dubins d = Dubins(a1, a2, radius, region1radius, region2radius); 42 | cout << "For radius=" << radius << " the shortest path (optimal GDIP solution) is:" << endl << d << endl; 43 | 44 | // Check if the endpoints are in the given disk-shaped regions. 45 | cout << "Chech the first endpoint (" << p1.distance(d.start.point) << " <= " << region1radius << ")." << endl; 46 | cout << "Chech the second endpoint (" << p2.distance(d.end.point) << " <= " << region2radius << ")." << endl; 47 | 48 | return 0; 49 | } -------------------------------------------------------------------------------- /gdip/examples/src/hellodubins.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "opendubins/dubins.h" 4 | 5 | using namespace std; 6 | using namespace opendubins; 7 | 8 | int main() { 9 | 10 | cout << "Hello, I am OpenDubins library!" << endl; 11 | Dubins d = Dubins(State(0, 0, 0), State(1, 1, 1), 1); 12 | cout << d << endl; 13 | 14 | return 0; 15 | } -------------------------------------------------------------------------------- /gdip/install.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | mkdir -p build 4 | cd build && cmake -D CMAKE_INSTALL_PREFIX=.. ../src .. 5 | cd - 6 | 7 | #env 8 | uname 9 | 10 | if [[ "$OS" == "Windows_NT" ]] ; then 11 | echo "Compiling on Windows" 12 | echo "cmake --build build --config Release" 13 | cmake --build build --config Release 14 | echo "cmake --install build --config Release" 15 | cmake --install build --config Release 16 | else 17 | echo "Compiling Linux/OSX" 18 | echo "make install -j4" 19 | make -C build install -j4 20 | fi 21 | -------------------------------------------------------------------------------- /gdip/wrappers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(opendubins_wrappers) 3 | 4 | include_directories("../core/src") 5 | include_directories("src") 6 | 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | 9 | set(SOURCE_FILES 10 | ../core/src/opendubins/arc.cpp 11 | ../core/src/opendubins/circle.cpp 12 | ../core/src/opendubins/dubins.cpp 13 | ../core/src/opendubins/dubinsmaneuver.cpp 14 | ../core/src/opendubins/dip.cpp 15 | ../core/src/opendubins/gdip.cpp 16 | ../core/src/opendubins/line.cpp 17 | ../core/src/opendubins/path.cpp 18 | ../core/src/opendubins/state.cpp 19 | ../core/src/opendubins/dubinspath.cpp 20 | ../core/src/opendubins/angleinterval.cpp 21 | 22 | src/gdipPythonAPI.cpp 23 | src/gdipJuliaAPI.cpp 24 | ) 25 | 26 | add_library(GDIP SHARED ${SOURCE_FILES}) 27 | 28 | install(TARGETS GDIP DESTINATION lib) 29 | -------------------------------------------------------------------------------- /gdip/wrappers/src/gdipJuliaAPI.cpp: -------------------------------------------------------------------------------- 1 | #include "gdipJuliaAPI.h" 2 | #include 3 | #include 4 | 5 | /************************* 6 | GDIP interface 7 | **************************/ 8 | 9 | GdipJuliaAPI juliaAPI; 10 | 11 | double julia_set_configurations( 12 | double p1x, double p1y, double t1, 13 | double p2x, double p2y, double t2, 14 | double radius) 15 | { 16 | State q1(p1x, p1y, t1); 17 | State q2(p2x, p2y, t2); 18 | juliaAPI.maneuver = Dubins(q1, q2, radius); 19 | return juliaAPI.maneuver.length; 20 | } 21 | 22 | double julia_set_configurations_dip( 23 | double p1x, double p1y, double t1left, double t1diff, 24 | double p2x, double p2y, double t2left, double t2diff, 25 | double radius) 26 | { 27 | AngleInterval int1(Point(p1x, p1y), t1left, t1diff); 28 | AngleInterval int2(Point(p2x, p2y), t2left, t2diff); 29 | juliaAPI.maneuver = Dubins(int1, int2, radius); 30 | return juliaAPI.maneuver.length; 31 | } 32 | 33 | double julia_set_configurations_gdip( 34 | double p1x, double p1y, double t1left, double t1diff, double radius1, 35 | double p2x, double p2y, double t2left, double t2diff, double radius2, 36 | double radius 37 | ) 38 | { 39 | AngleInterval int1(Point(p1x, p1y), t1left, t1diff); 40 | AngleInterval int2(Point(p2x, p2y), t2left, t2diff); 41 | juliaAPI.maneuver = Dubins(int1, int2, radius, radius1, radius2); 42 | return juliaAPI.maneuver.length; 43 | } 44 | 45 | double julia_get_length(){ 46 | return juliaAPI.maneuver.length; 47 | } 48 | 49 | void julia_sample_state_to_tmp(double distance){ 50 | juliaAPI.tmp_state = juliaAPI.maneuver.getState(distance); 51 | } 52 | 53 | double julia_get_tmp_x(){ 54 | return juliaAPI.tmp_state.point.x; 55 | } 56 | 57 | double julia_get_tmp_y(){ 58 | return juliaAPI.tmp_state.point.y; 59 | } 60 | 61 | double julia_get_tmp_theta(){ 62 | return juliaAPI.tmp_state.ang; 63 | } 64 | 65 | bool julia_dubins_circle_intersection( 66 | double circle_origin_x, double circle_origin_y, double circle_radius 67 | ){ 68 | Circle c(Point(circle_origin_x, circle_origin_y), circle_radius); 69 | return juliaAPI.maneuver.intersectCircle(c).isValid(); 70 | } 71 | 72 | void julia_dubins_closest(double point_x, double point_y){ 73 | auto state_at_distance = juliaAPI.maneuver.getClosestStateAndDistance(Point(point_x, point_y)); 74 | juliaAPI.tmp_state = state_at_distance.state; 75 | } -------------------------------------------------------------------------------- /gdip/wrappers/src/gdipJuliaAPI.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "opendubins/dubins.h" 4 | 5 | using namespace opendubins; 6 | 7 | struct GdipJuliaAPI{ 8 | Dubins maneuver; 9 | State tmp_state; 10 | }; 11 | 12 | #ifdef __cplusplus 13 | extern "C" { 14 | #endif 15 | 16 | double julia_set_configurations( 17 | double p1x, double p1y, double t1, 18 | double p2x, double p2y, double t2, 19 | double radius 20 | ); 21 | 22 | double julia_set_configurations_dip( 23 | double p1x, double p1y, double t1left, double t1diff, 24 | double p2x, double p2y, double t2left, double t2diff, 25 | double radius 26 | ); 27 | 28 | double julia_set_configurations_gdip( 29 | double p1x, double p1y, double t1left, double t1diff, double radius1, 30 | double p2x, double p2y, double t2left, double t2diff, double radius2, 31 | double radius 32 | ); 33 | 34 | double julia_get_length(); 35 | 36 | void julia_sample_state_to_tmp(double distance); 37 | 38 | double julia_get_tmp_x(); 39 | double julia_get_tmp_y(); 40 | double julia_get_tmp_theta(); 41 | 42 | bool julia_dubins_circle_intersection( 43 | double circle_origin_x, double circle_origin_y, double circle_radius 44 | ); 45 | 46 | void julia_dubins_closest( 47 | double point_x, double point_y 48 | ); 49 | 50 | #ifdef __cplusplus 51 | } 52 | #endif 53 | 54 | -------------------------------------------------------------------------------- /gdip/wrappers/src/gdipPythonAPI.cpp: -------------------------------------------------------------------------------- 1 | #include "gdipPythonAPI.h" 2 | #include 3 | #include 4 | 5 | /************************* 6 | GDIP interface 7 | **************************/ 8 | 9 | GdipPythonAPI* new_GdipAPI(){ 10 | return new GdipPythonAPI(); 11 | } 12 | 13 | void python_set_configurations(GdipPythonAPI* api, double start[3], double end[3], double radius){ 14 | State q1(start[0], start[1], start[2]); 15 | State q2(end[0], end[1], end[2]); 16 | api->maneuver = Dubins(q1, q2, radius); 17 | } 18 | 19 | void python_set_configurations_dip(GdipPythonAPI* api, double point1[2], double interval1[2], double point2[2], double interval2[2], double radius){ 20 | AngleInterval int1(Point(point1[0], point1[1]), interval1[0], interval1[1]); 21 | AngleInterval int2(Point(point2[0], point2[1]), interval2[0], interval2[1]); 22 | api->maneuver = Dubins(int1, int2, radius); 23 | } 24 | 25 | void python_set_configurations_gdip(GdipPythonAPI* api, double point1[2], double interval1[2], double radius1, double point2[2], double interval2[2], double radius2, double radius){ 26 | AngleInterval int1(Point(point1[0], point1[1]), interval1[0], interval1[1]); 27 | AngleInterval int2(Point(point2[0], point2[1]), interval2[0], interval2[1]); 28 | api->maneuver = Dubins(int1, int2, radius, radius1, radius2); 29 | } 30 | 31 | 32 | double python_get_length(GdipPythonAPI* api){ 33 | return api->maneuver.length; 34 | } 35 | 36 | void python_sample_state_to_tmp(GdipPythonAPI* api, double distance){ 37 | api->tmp_state = api->maneuver.getState(distance); 38 | } 39 | 40 | double python_get_tmp_x(GdipPythonAPI* api){ 41 | return api->tmp_state.point.x; 42 | } 43 | 44 | double python_get_tmp_y(GdipPythonAPI* api){ 45 | return api->tmp_state.point.y; 46 | } 47 | 48 | double python_get_tmp_theta(GdipPythonAPI* api){ 49 | return api->tmp_state.ang; 50 | } 51 | 52 | void python_destruct(GdipPythonAPI* api){ 53 | delete api; 54 | } 55 | -------------------------------------------------------------------------------- /gdip/wrappers/src/gdipPythonAPI.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "opendubins/dubins.h" 4 | 5 | using namespace opendubins; 6 | 7 | struct GdipPythonAPI{ 8 | Dubins maneuver; 9 | State tmp_state; 10 | }; 11 | 12 | 13 | #ifdef __cplusplus 14 | extern "C" { 15 | #endif 16 | 17 | GdipPythonAPI* new_GdipAPI(); 18 | 19 | void python_set_configurations(GdipPythonAPI* api, double start[3], double end[3], double radius); 20 | void python_set_configurations_dip(GdipPythonAPI* api, double point1[2], double interval1[2], double point2[2], double interval2[2], double radius); 21 | void python_set_configurations_gdip(GdipPythonAPI* api, double point1[2], double interval1[2], double radius1, double point2[2], double interval2[2], double radius2, double radius); 22 | 23 | double python_get_length(GdipPythonAPI* api); 24 | 25 | void python_sample_state_to_tmp(GdipPythonAPI* api, double distance); 26 | 27 | double python_get_tmp_x(GdipPythonAPI* api); 28 | double python_get_tmp_y(GdipPythonAPI* api); 29 | double python_get_tmp_theta(GdipPythonAPI* api); 30 | 31 | void python_destruct(GdipPythonAPI* api); 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif 36 | 37 | -------------------------------------------------------------------------------- /julia/GDIP.jl: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env julia 2 | 3 | # Wrapper for GDIP library libGDIP.so 4 | 5 | using Libdl 6 | 7 | # Open the library explicitly. 8 | lib = Libdl.dlopen("$(@__DIR__)/../gdip/lib/libGDIP") 9 | 10 | @inline function gdip_get_length() 11 | return ccall( (:julia_get_length, :libGDIP), Cdouble, () ) 12 | end 13 | 14 | @inline function gdip_sample_many(step::Float64) 15 | length = gdip_get_length() 16 | samples = Vector{Vector{Float64}}() 17 | sample_l = collect(0:step:length) 18 | push!(sample_l, length) 19 | for l in sample_l 20 | ccall( (:julia_sample_state_to_tmp, :libGDIP), Cvoid, (Cdouble,), l) 21 | x = ccall( (:julia_get_tmp_x, :libGDIP), Cdouble, (), ) 22 | y = ccall( (:julia_get_tmp_y, :libGDIP), Cdouble, (), ) 23 | theta = ccall( (:julia_get_tmp_theta, :libGDIP), Cdouble, (), ) 24 | push!(samples, [x,y,theta]) 25 | end 26 | return samples 27 | end 28 | 29 | @inline function gdip_init_dubins_maneuver( 30 | from::Array{Float64,1}, to::Array{Float64,1}, turningRadius::Float64 31 | ) 32 | length = ccall( 33 | (:julia_set_configurations, :libGDIP), # name of C function and library 34 | Cdouble, # output type 35 | (Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble), # tuple of input types 36 | from[1], from[2], from[3], to[1], to[2], to[3], turningRadius # names of Julia variables to pass in 37 | ) 38 | return length 39 | end 40 | 41 | @inline function gdip_init_dip( 42 | p1::Array{Float64,1}, interval1::Array{Float64,1}, 43 | p2::Array{Float64,1}, interval2::Array{Float64,1}, 44 | turningRadius::Float64 45 | ) 46 | length = ccall( 47 | (:julia_set_configurations_dip, :libGDIP), # name of C function and library 48 | Cdouble, # output type 49 | (Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble), # tuple of input types 50 | p1[1], p1[2], interval1[1], interval1[2], p2[1], p2[2], interval2[1], interval2[2], turningRadius # names of Julia variables to pass in 51 | ) 52 | return length 53 | end 54 | 55 | @inline function gdip_init_gdip( 56 | p1::Array{Float64,1}, interval1::Array{Float64,1}, radius1::Float64, 57 | p2::Array{Float64,1}, interval2::Array{Float64,1}, radius2::Float64, 58 | turningRadius::Float64 59 | ) 60 | length = ccall( 61 | (:julia_set_configurations_gdip, :libGDIP), # name of C function and library 62 | Cdouble, # output type 63 | (Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble, Cdouble), # tuple of input types 64 | p1[1], p1[2], interval1[1], interval1[2], radius1, p2[1], p2[2], interval2[1], interval2[2], radius2, turningRadius # names of Julia variables to pass in 65 | ) 66 | return length 67 | end 68 | 69 | @inline function gdip_dubins_circle_intersection( 70 | circle_origin::Array{Float64,1}, circle_radius::Float64 71 | ) 72 | isintersecting = ccall( 73 | (:julia_dubins_circle_intersection, :libGDIP), # name of C function and library 74 | Cuchar, # output type 75 | (Cdouble, Cdouble, Cdouble), # circle_origin_x, circle_origin_y, circle_radius 76 | circle_origin[1], circle_origin[2], circle_radius # names of Julia variables to pass in 77 | ) 78 | return isintersecting > 0 79 | end 80 | 81 | @inline function gdip_dubins_closest(point::Array{Float64,1}) 82 | ccall( 83 | (:julia_dubins_closest, :libGDIP), # name of C function and library 84 | Cvoid, # output type 85 | (Cdouble, Cdouble), # point_x, point_y 86 | point[1], point[2] # names of Julia variables to pass in 87 | ) 88 | x = ccall( (:julia_get_tmp_x, :libGDIP), Cdouble, (), ) 89 | y = ccall( (:julia_get_tmp_y, :libGDIP), Cdouble, (), ) 90 | theta = ccall( (:julia_get_tmp_theta, :libGDIP), Cdouble, (), ) 91 | return [x,y,theta] 92 | end 93 | -------------------------------------------------------------------------------- /julia/basic_gdip_example.jl: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env julia 2 | 3 | # Test for Wrapper for GDIP library libGDIP.so 4 | 5 | ################################################## 6 | # Imports 7 | ################################################## 8 | 9 | using Printf 10 | 11 | # import PyPlot in version 2.8.0+ 12 | import Pkg 13 | try 14 | using PyPlot 15 | catch 16 | Pkg.add("PyPlot") 17 | using PyPlot 18 | end 19 | if PyPlot.version < v"2.8.0" 20 | error("Please upgrade PyPlot to version 2.8.0+") 21 | @assert false 22 | end 23 | Circle = matplotlib.patches.Circle 24 | 25 | # import wrapper for the GDIP solution 26 | include("GDIP.jl") 27 | 28 | ################################################## 29 | # Settings 30 | ################################################## 31 | 32 | # Turning radius 33 | turning_radius = 1.0 34 | # Step of the theta2 angle in the visualization 35 | step = 2 * pi / 100 36 | # Wait time for the animation 37 | wait_time = 0.01 38 | # Save figures into a directory "images" 39 | save_figures = false 40 | if length(ARGS) > 0 41 | if "-save" == ARGS[1] 42 | save_figures = true 43 | end 44 | end 45 | if save_figures 46 | mkpath("images") 47 | end 48 | # Sensing radius in case of GDIP solution 49 | sensing_radius_gdip = 0.5 50 | 51 | # End points 52 | point1 = [0.0, 0.0] 53 | point2 = [5.0, 0.0] 54 | 55 | # First angle interval (the second is changed) 56 | interval1 = [1.0, 0.5] 57 | 58 | # Enable only specific parts 59 | DUB = false 60 | DIP = false 61 | GDIP = true 62 | 63 | #matplotlib.rcParams['text.usetex'] = true 64 | 65 | ################################################## 66 | # Functions 67 | ################################################## 68 | 69 | function plot_points(points; specs = "b", zorder = 1) 70 | x_val = [x[1] for x in points] 71 | y_val = [x[2] for x in points] 72 | return plt.plot(x_val, y_val, specs, zorder = zorder) 73 | end 74 | 75 | function plot_interval(point, interval, sensing_radius) 76 | ax = plt.gca() 77 | circle = Circle(point, sensing_radius, facecolor="yellow", edgecolor="orange", linewidth=1, alpha=0.2) 78 | ax.add_patch(circle) 79 | 80 | for angle in [interval[1], interval[1] + interval[2]] 81 | last = point + turning_radius * [cos(angle), sin(angle)] 82 | points = [point, last] 83 | plot_points(points, specs = "y-", zorder = 1) 84 | end 85 | end 86 | 87 | function plot_arrow(px, py, angle; alpha = 1) 88 | rad = 0.7 * turning_radius 89 | dx = rad * cos(angle) 90 | dy = rad * sin(angle) 91 | plt.arrow(px, py, dx, dy, head_width=0.05, head_length=0.1, fc="k", ec="k", zorder=2, alpha=alpha) 92 | plt.plot([px, px + dx], [py, py + dy], "k", visible=false) 93 | end 94 | 95 | function plot_figure(samples, point1, point2, title; sensing_radius = 0, interval1 = nothing, interval2 = nothing) 96 | # Clear and basic settings 97 | plt.clf() 98 | plt.axis("equal") 99 | plt.title(title) 100 | plt.xlabel("x [-]") 101 | plt.ylabel("y [-]") 102 | plt.ylim([-3, 3]) 103 | plt.xlim([-1.5, 6.5]) 104 | plt.grid(alpha=0.2) 105 | 106 | # Plot regions based on sensing radius 107 | if interval1 != nothing 108 | plot_interval(point1, interval1, sensing_radius) 109 | end 110 | if interval2 != nothing 111 | plot_interval(point2, interval2, sensing_radius) 112 | end 113 | 114 | # Plot samples of the Dubins maneuver 115 | ps = plot_points(samples) 116 | plt.legend([ps[1]], ["Dubins maneuver"]) 117 | 118 | # First arrow 119 | p1 = samples[1] 120 | p2 = samples[2] 121 | angle = atan(p2[2]-p1[2], p2[1]-p1[1]) 122 | plot_arrow(p1[1], p1[2], angle) 123 | plot_arrow(point1[1], point1[2], angle, alpha = 0.3) 124 | plt.plot(p1[1], p1[2], "k.") 125 | 126 | # Second arrow 127 | p1 = samples[length(samples)] 128 | p2 = samples[length(samples)-1] 129 | angle = atan(p1[2]-p2[2], p1[1]-p2[1]) 130 | plot_arrow(p1[1], p1[2], angle) 131 | plot_arrow(point2[1], point2[2], angle, alpha = 0.3) 132 | plt.plot(p1[1], p1[2], "k.") 133 | 134 | if ! save_figures 135 | plt.pause(wait_time) 136 | end 137 | 138 | if save_figures 139 | filename = @sprintf("images/%05d.png", image_idx) 140 | #filename = "images/{:05d}.png".format(image_idx) 141 | global image_idx = image_idx + 1 142 | print(filename * "\n") 143 | plt.savefig(filename) 144 | end 145 | end 146 | 147 | image_idx = 0 148 | 149 | ################################################## 150 | # Basic Dubins maneuver 151 | ################################################## 152 | if DUB 153 | for t2 in 0:step:(2*pi) 154 | start = [point1[1], point1[2], 1.0] 155 | final = [point2[1], point2[2], t2] 156 | gdip_init_dubins_maneuver(start, final, 1.0) 157 | samples = gdip_sample_many(step_size) 158 | 159 | plot_figure(samples, point1, point2, "Dubins maneuver") 160 | end 161 | end 162 | 163 | ################################################## 164 | # Dubins Interval Problem (DIP) 165 | ################################################## 166 | if DIP 167 | sensing_radius = 0.0 168 | 169 | for t2 in 0:step:(2*pi) 170 | interval2 = [2 + t2, 0.5] 171 | gdip_init_dip(point1, interval1, point2, interval2, turning_radius) 172 | samples = gdip_sample_many(0.1) 173 | 174 | plot_figure(samples, point1, point2, "Dubins Interval Problem (DIP)", 175 | sensing_radius = sensing_radius, interval1 = interval1, interval2 = interval2) 176 | 177 | end 178 | end 179 | 180 | ################################################## 181 | # Generalized Dubins Interval Problem (GDIP) 182 | ################################################## 183 | if GDIP 184 | sensing_radius = sensing_radius_gdip 185 | 186 | for t2 in 0:step:(2*pi) 187 | interval2 = [2 + t2, 0.5] 188 | 189 | gdip_init_gdip(point1, interval1, sensing_radius, point2, interval2, sensing_radius, turning_radius) 190 | samples = gdip_sample_many(0.1) 191 | 192 | plot_figure(samples, point1, point2, "Dubins Interval Problem (DIP)", 193 | sensing_radius = sensing_radius, interval1 = interval1, interval2 = interval2) 194 | 195 | end 196 | end 197 | -------------------------------------------------------------------------------- /julia/create_gif_example.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Basic GDIP example 4 | rm -rf images 5 | ./basic_gdip_example.jl -save 6 | convert -delay 7 -loop 0 images/*.png basic-gdip-example.gif 7 | convert basic-gdip-example.gif -fuzz 5% -layers Optimize basic-gdip-example-small.gif 8 | 9 | # RSS'18 - DTRP example 10 | rm -rf images 11 | ./rss18_dtrp.py -save 12 | convert -delay 50 -loop 0 images/*.png rss-example.gif 13 | convert rss-example.gif -fuzz 5% -layers Optimize rss-example-small.gif 14 | -------------------------------------------------------------------------------- /julia/problems/gdip-n10.txt: -------------------------------------------------------------------------------- 1 | 1 6.71627 8.3767 2 | 2 6.40217 9.49307 3 | 3 3.41556 10.2848 4 | 4 5.91928 7.20563 5 | 5 3.90768 3.7961 6 | 6 7.52977 3.70221 7 | 7 8.84408 3.80583 8 | 8 10.6659 6.6348 9 | 9 12.1318 7.59114 10 | 10 10.5947 10.1087 11 | -------------------------------------------------------------------------------- /julia/rss18_dtrp.jl: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env julia 2 | 3 | ################################################## 4 | # Imports 5 | ################################################## 6 | 7 | using Printf 8 | 9 | # import PyPlot in version 2.8.0+ 10 | import Pkg 11 | try 12 | using PyPlot 13 | catch 14 | Pkg.add("PyPlot") 15 | using PyPlot 16 | end 17 | if PyPlot.version < v"2.8.0" 18 | error("Please upgrade PyPlot to version 2.8.0+") 19 | end 20 | Circle = matplotlib.patches.Circle 21 | 22 | try 23 | using TimerOutputs 24 | catch 25 | Pkg.add("TimerOutputs") 26 | using TimerOutputs 27 | end 28 | # Create the timer object 29 | to = TimerOutput() 30 | 31 | # import wrapper for the GDIP solution 32 | include("GDIP.jl") 33 | 34 | ################################################ 35 | # Scenarios 36 | ################################################ 37 | #define planning problems: 38 | # map file 39 | # minimum turning radius 40 | # sensing radius 41 | # solver type 42 | scenarios = [ 43 | ("./problems/gdip-n10.txt", 1.0, 1.0, "DTSPN-GDIP"), 44 | #("./problems/gdip-n10.txt", 1.0, 0.5, "DTSPN-GDIP"), 45 | #("./problems/gdip-n10.txt", 0.5, 1.0, "DTSPN-GDIP"), 46 | ] 47 | 48 | ################################################ 49 | # Settings 50 | ################################################ 51 | 52 | visualize = true 53 | show = true 54 | 55 | # Save figures into a directory "images" 56 | save_figures = false 57 | if length(ARGS) > 0 58 | if "-save" == ARGS[1] 59 | visualize = true 60 | save_figures = true 61 | end 62 | end 63 | if save_figures 64 | mkpath("images") 65 | end 66 | 67 | ################################################## 68 | # Functions 69 | ################################################## 70 | 71 | function load_map(filename) 72 | """Read config with goal positions""" 73 | goals = [] 74 | lines = readlines(open(filename)) 75 | for line in lines 76 | label, x, y = split(line) 77 | push!(goals, [parse(Float64,x), parse(Float64,y)]) 78 | end 79 | return goals 80 | end 81 | 82 | function plot_points(points; specs = "b") 83 | x_val = [x[1] for x in points] 84 | y_val = [x[2] for x in points] 85 | plt.plot(x_val, y_val, specs) 86 | end 87 | 88 | function plot_circle(xy, radius) 89 | ax = plt.gca() 90 | circle = Circle(xy, radius, facecolor="yellow",edgecolor="orange", linewidth=1, alpha=0.2) 91 | ax.add_patch(circle) 92 | end 93 | 94 | function dist_euclidean(coord1, coord2) 95 | (x1, y1) = coord1 96 | (x2, y2) = coord2 97 | (dx, dy) = (x2 - x1, y2 - y1) 98 | return sqrt(dx * dx + dy * dy) 99 | end 100 | 101 | 102 | # cache results from computing length of the GDIP 103 | lowerPathGDIPLenCache = Dict() 104 | 105 | function lowerPathGDIP(s1, s2, turning_radius; cached = false) 106 | """Compute lower-bound path using GDIP between two configurations 107 | 108 | Arguments: 109 | s1 - start; s2 - end; turning_radius 110 | cached - compute only distance and cache the results 111 | Returns: 112 | Dubins maneuver (DubinsWrapper) 113 | """ 114 | #if cached: 115 | # key = (s1, s2) 116 | # if key in lowerPathGDIPLenCache: 117 | # length = lowerPathGDIPLenCache[key] 118 | # return (None, length) 119 | 120 | interval1 = [s1.alpha1, s1.alpha2 - s1.alpha1] 121 | interval2 = [s2.alpha1, s2.alpha2 - s2.alpha1] 122 | #@timeit to "gdip" 123 | return gdip_init_gdip(s1.center, interval1, s1.radius, s2.center, interval2, s2.radius, turning_radius) 124 | 125 | #if cached: 126 | # lowerPathGDIPLenCache[key] = length 127 | # return (None, length) 128 | #else: 129 | # return (path, length) 130 | return length 131 | end 132 | 133 | function upperPathGDIP(s1, s2, turning_radius; cached = false) 134 | """Compute feasible Dubins path two configurations 135 | 136 | Arguments: 137 | s1 - start; s2 - end; turning_radius 138 | Returns: 139 | (Dubins maneuver 'DubinsWrapper', length) 140 | """ 141 | q1 = getFeasibleState(s1) 142 | q2 = getFeasibleState(s2) 143 | return gdip_init_dubins_maneuver(q1, q2, turning_radius) 144 | end 145 | 146 | function compute_distances(samples, dst_fce; turning_radius = 0) 147 | n = length(samples) 148 | distances = [] 149 | 150 | for i in 1:n 151 | ss1 = samples[i] 152 | ss2 = samples[(i % n) + 1] 153 | 154 | n1 = length(ss1) 155 | n2 = length(ss2) 156 | 157 | sh = fill(Inf, (n1, n2)) 158 | for i1 in 1:n1 159 | for i2 in 1:n2 160 | sh[i1,i2] = dst_fce(ss1[i1], ss2[i2], turning_radius, cached=true) 161 | end 162 | end 163 | push!(distances, sh) 164 | end 165 | 166 | return distances 167 | end 168 | 169 | function update_distances(samples, distances, selected_samples, dst_fce; turning_radius = 0) 170 | n = length(samples) 171 | for i in 1:n 172 | ss1 = samples[i] 173 | ss2 = samples[(i % n) + 1] 174 | 175 | n1 = length(ss1) 176 | n2 = length(ss2) 177 | 178 | sh_old = distances[i] 179 | siz_old = size(sh_old) 180 | sh = fill(NaN, (n1, n2)) 181 | sh[1:siz_old[1],1:siz_old[2]] = sh_old 182 | 183 | #@show size(sh), selected_samples[i], sh[selected_samples[i], :] 184 | sh[selected_samples[i], :] *= NaN 185 | sh[:,selected_samples[(i%n)+1]] *= NaN 186 | 187 | # Alternative without going throw the whole distance matrix 188 | #= 189 | for j in 1:n1 190 | dist = dst_fce(ss1[j], ss2[n2], turning_radius, cached=true)[1] 191 | sh[j,n2] = dist 192 | end 193 | 194 | for j in 1:n2 195 | dist = dst_fce(ss1[n1], ss2[j], turning_radius, cached=true)[1] 196 | sh[n1,j] = dist 197 | end 198 | 199 | i1 = selected_samples[i] 200 | for i2 in 1:n2 201 | dist = dst_fce(ss1[i1], ss2[i2], turning_radius, cached=true)[1] 202 | sh[i1, i2] = dist 203 | end 204 | 205 | i2 = selected_samples[(i%n)+1] 206 | for i1 in 1:n1 207 | dist = dst_fce(ss1[i1], ss2[i2], turning_radius, cached=true)[1] 208 | sh[i1, i2] = dist 209 | end 210 | =# 211 | 212 | for i1 in 1:n1 213 | for i2 in 1:n2 214 | dist = sh[i1,i2] 215 | if isnan(dist) 216 | @timeit to "dist_fce" begin 217 | sh[i1,i2] = dst_fce(ss1[i1], ss2[i2], turning_radius, cached=true) 218 | end 219 | end 220 | end 221 | end 222 | 223 | distances[i] = sh 224 | end 225 | 226 | return distances 227 | end 228 | 229 | function find_shortest_tour(distances) 230 | n = size(distances)[1] 231 | best_len = Inf 232 | best_tour = [] 233 | 234 | # TODO - remove 235 | #global dist = distances 236 | 237 | # maximal number of samples 238 | k_max = maximum([size(x)[1] for x in distances]) 239 | 240 | no_start = size(distances[1])[1] 241 | #@show no_start, k_max, n 242 | for start in 1:no_start 243 | # shortest sh[region_idx][sample_idx] 244 | sh = fill(Inf, (n+1, k_max)) 245 | # used edge 246 | prev = fill(-1, (n+1, k_max)) 247 | 248 | sh[1,start] = 0 249 | 250 | for region_idx in 1:n 251 | (n1, n2) = size(distances[region_idx]) 252 | #@show n1, n2, distances[region_idx] 253 | for idx2 in 1:n2 254 | #@show distances[region_idx] 255 | #@show sh[region_idx,1:n1], distances[region_idx][:,idx2] 256 | dst = sh[region_idx,1:n1] + distances[region_idx][:,idx2] 257 | sh[region_idx+1,idx2] = minimum(dst) 258 | #@show dst 259 | #@show minimum(dst) 260 | prev[region_idx+1,idx2]= argmin(dst) 261 | end 262 | end 263 | 264 | #@show size(sh), n, start 265 | act_sol = sh[n+1,start] 266 | if act_sol < best_len 267 | best_len = act_sol 268 | tour = [] 269 | act = start 270 | for i in 1:n 271 | act = prev[n-i+2,act] 272 | push!(tour, act) 273 | end 274 | best_tour = reverse(tour) 275 | #print(best_tour) 276 | end 277 | end 278 | 279 | return best_tour 280 | end 281 | 282 | function retrieve_path(samples, dst_fce, turning_radius, selected_samples) 283 | n = length(samples) 284 | path = [] 285 | for a in 1:n 286 | g1 = samples[a][selected_samples[a]] 287 | g2 = samples[(a+1) % n][selected_samples[(a+1) % n]] 288 | path.append(dst_fce(g1, g2, turning_radius)) 289 | end 290 | return path 291 | end 292 | 293 | function path_len(path) 294 | return sum_list([dub.get_length() for dub in path]) 295 | end 296 | 297 | function plot_path(path, turning_radius, settings) 298 | step_size = 0.01 * turning_radius 299 | for dub in path 300 | configurations, _ = dub.sample_many(step_size) 301 | plot_points(configurations, settings) 302 | end 303 | end 304 | 305 | ################################################## 306 | # Target region given by the location and its sensing radius 307 | ################################################## 308 | mutable struct TargetRegion 309 | center 310 | radius 311 | end 312 | 313 | function get_position_at_boundary(region::TargetRegion, beta) 314 | return region.center + region.radius * [cos(beta), sin(beta)] 315 | end 316 | 317 | ################################################## 318 | # Sample on the given target region 319 | ################################################## 320 | mutable struct Sample 321 | target 322 | 323 | center 324 | radius 325 | 326 | alpha1 327 | alpha2 328 | alphaResolution 329 | 330 | beta1 331 | beta2 332 | betaResolution 333 | end 334 | 335 | Base.copy(s::Sample) = Sample(s.target, s.center, s.radius, s.alpha1, s.alpha2, s.alphaResolution, s.beta1, s.beta2, s.betaResolution) 336 | 337 | function Sample_init(targetRegion) 338 | alpha1 = 0 339 | alpha2 = 2 * pi 340 | alphaResolution = 1 341 | 342 | beta1 = 0 343 | beta2 = 2 * pi 344 | betaResolution = 1 345 | 346 | return Sample(targetRegion, targetRegion.center, targetRegion.radius, alpha1, alpha2, alphaResolution, beta1, beta2, betaResolution) 347 | end 348 | 349 | # TODO missing functions 350 | 351 | function Sample_plot(sample) 352 | ax = plt.gca() 353 | circle = Circle(sample.center, sample.radius, facecolor=nothing ,edgecolor="green", linewidth=1, alpha=0.2) 354 | ax.add_patch(circle) 355 | end 356 | 357 | function Sample_split(sample, resolution) 358 | """Split the actual sample into two new ones. 359 | The first is stored directly in the actual sample, and the second is returned. 360 | If the required resolution is already met, then nothing is done and None is returned. 361 | 362 | Parameters: 363 | resolution: the requred resolution 364 | Returns: 365 | Sample - the second new sample 366 | """ 367 | # prefer splitting according position resolution 368 | if sample.betaResolution < resolution 369 | sam1 = copy(sample) 370 | sam2 = copy(sample) 371 | sam1.betaResolution = sam2.betaResolution = 2 * sample.betaResolution 372 | sam1.beta2 = sam2.beta1 = (sample.beta1 + sample.beta2) / 2 373 | update_center_radius(sam1) 374 | update_center_radius(sam2) 375 | return [sam1, sam2] 376 | end 377 | if sample.alphaResolution < resolution 378 | sam1 = copy(sample) 379 | sam2 = copy(sample) 380 | sam1.alphaResolution = sam2.alphaResolution = 2 * sample.alphaResolution 381 | sam1.alpha2 = sam2.alpha1 = (sample.alpha1 + sample.alpha2) / 2 382 | return [sam1, sam2] 383 | end 384 | return nothing 385 | end 386 | 387 | function update_center_radius(self) 388 | p1 = get_position_at_boundary(self.target, self.beta1) 389 | p2 = get_position_at_boundary(self.target, self.beta2) 390 | self.center = (p1 + p2) / 2 391 | self.radius = dist_euclidean(p1, p2) / 2 392 | end 393 | 394 | function getFeasibleState(self) 395 | pos = get_position_at_boundary(self.target, self.beta1) 396 | q = zeros(3) 397 | q[1:2] = pos 398 | q[3] = self.alpha1 399 | return q 400 | end 401 | 402 | ################################################## 403 | # Sampling structure which holds all the used samples 404 | ################################################## 405 | mutable struct Sampling 406 | targets 407 | samples 408 | end 409 | 410 | function Sampling_init(centers, sensingRadius) 411 | targets = [TargetRegion(c, sensingRadius) for c in centers] 412 | samples = [[Sample_init(t)] for t in targets] 413 | return Sampling(targets, samples) 414 | end 415 | 416 | function refine_samples(self, selected, resolution) 417 | """Refine the seleted samples if the required resolution is not met. 418 | 419 | Parameters: 420 | slected: indexes of the selected samples (vector 1 x n) 421 | resolution: the requred resolution 422 | Returns: 423 | boolean - true if any sample is refined 424 | """ 425 | n = length(self.samples) 426 | refined = false 427 | for i in 1:n 428 | to_split = selected[i] 429 | samp = self.samples[i][to_split] 430 | res = Sample_split(samp, resolution) 431 | if res != nothing 432 | self.samples[i][to_split] = res[1] 433 | push!(self.samples[i], res[2]) 434 | refined = true 435 | end 436 | end 437 | return refined 438 | end 439 | 440 | ################################################## 441 | # The main solver class 442 | ################################################## 443 | mutable struct GDIPSolver 444 | turning_radius::Float64 445 | sensing_radius::Float64 446 | 447 | sampling::Sampling 448 | 449 | goals 450 | 451 | lower_path::Array{Integer,1} 452 | upper_path::Array{Integer,1} 453 | 454 | lower_bound::Float64 455 | upper_bound::Float64 456 | end 457 | 458 | function GDIPSolver_init(turning_radius, goals, sensing_radius) 459 | return GDIPSolver(turning_radius, sensing_radius, Sampling_init(goals, sensing_radius), goals, [], [], 0, Inf) 460 | end 461 | 462 | function GDIPSolver_plot_map(solver) 463 | plt.clf() 464 | plt.axis("equal") 465 | plot_points(solver.goals, specs = "ro") 466 | if solver.sensing_radius != 0 467 | for goal in solver.goals 468 | plot_circle(goal, solver.sensing_radius) 469 | end 470 | end 471 | end 472 | 473 | function GDIPSolver_plot_tour_and_return_length(solver, selected_samples, maneuver_function, color) 474 | sampling = solver.sampling 475 | n = length(solver.sampling.samples) 476 | step_size = 0.01 * solver.turning_radius 477 | len = 0 478 | for a in 1:n 479 | b = (a%n)+1 480 | g1 = sampling.samples[a][selected_samples[a]] 481 | g2 = sampling.samples[b][selected_samples[b]] 482 | 483 | len += maneuver_function(g1, g2, solver.turning_radius) 484 | configurations = gdip_sample_many(step_size) 485 | if visualize 486 | plot_points(configurations, specs = color) 487 | end 488 | end 489 | return len 490 | end 491 | 492 | function GDIPSolver_plot_actual_and_return_bounds(solver) 493 | """Plot the actual sampling, lower and upper bound path 494 | 495 | Returns: 496 | (double, double) - lower bound, upper bound 497 | """ 498 | if visualize 499 | GDIPSolver_plot_map(solver) 500 | end 501 | 502 | for s in solver.sampling.samples 503 | for ss in s 504 | Sample_plot(ss) 505 | end 506 | end 507 | 508 | lower_selected_samples = GDIPSolver_find_shortest_tour(solver, lowerPathGDIP)[1] 509 | upper_selected_samples = GDIPSolver_find_shortest_tour(solver, upperPathGDIP)[1] 510 | 511 | lower_bound = GDIPSolver_plot_tour_and_return_length(solver, lower_selected_samples, lowerPathGDIP, "r-") 512 | upper_bound = GDIPSolver_plot_tour_and_return_length(solver, upper_selected_samples, upperPathGDIP, "b-") 513 | return (lower_bound, upper_bound) 514 | end 515 | 516 | function GDIPSolver_find_shortest_tour(solver, fce; dist = nothing, selected = nothing) 517 | """Select the samples which represent the shortest lower bound tour 518 | 519 | Returns: 520 | indexes of the samples (vector 1 x n) 521 | """ 522 | if dist == nothing || selected == nothing 523 | distances = compute_distances(solver.sampling.samples, fce, turning_radius = solver.turning_radius) 524 | else 525 | @timeit to "Update" distances = update_distances(solver.sampling.samples, dist, selected, fce, turning_radius = solver.turning_radius) 526 | end 527 | @timeit to "Find" selected_samples = find_shortest_tour(distances) 528 | return selected_samples, distances 529 | end 530 | 531 | ################################################## 532 | # Main loop over selected scenarios 533 | ################################################## 534 | for scenario in scenarios 535 | # Load the problem and scenario settings 536 | filename = scenario[1] 537 | goals = load_map(filename) 538 | turning_radius = scenario[2] 539 | sensing_radius = scenario[3] 540 | solver_type = scenario[4] 541 | 542 | #tour planning part 543 | solver = GDIPSolver_init(turning_radius, goals, sensing_radius) 544 | GDIPSolver_plot_actual_and_return_bounds(solver) 545 | 546 | @printf("\n--- Problem: %s Turning radius: %6.2f Sensing radius: %6.2f ---\n", 547 | filename, turning_radius, sensing_radius) 548 | 549 | if show 550 | plt.pause(0.1) 551 | end 552 | 553 | max_resolution = 64 554 | act_res = 4 555 | while act_res <= max_resolution 556 | refined = true 557 | distances = nothing 558 | selected_samples = nothing 559 | @timeit to "Iteration" begin 560 | while refined 561 | @timeit to "Tour" selected_samples, distances = GDIPSolver_find_shortest_tour( 562 | solver, lowerPathGDIP, dist = distances, selected = selected_samples) 563 | @timeit to "Refine" refined = refine_samples(solver.sampling, selected_samples, act_res) 564 | end 565 | end 566 | (lower_bound, upper_bound) = GDIPSolver_plot_actual_and_return_bounds(solver) 567 | gap = (upper_bound - lower_bound) / upper_bound * 100.0 568 | @printf("Res: %4d Lower: %6.2f Upper: %6.2f Gap(%%): %6.2f\n", 569 | act_res, lower_bound, upper_bound, gap) 570 | 571 | 572 | 573 | if visualize 574 | plt.title(@sprintf("Maximum resolution: %4d", act_res)) 575 | end 576 | 577 | if show 578 | plt.pause(0.1) 579 | end 580 | 581 | if save_figures 582 | plt.savefig(@sprintf("images/dtrp-res-%04d.png", act_res)) 583 | end 584 | 585 | act_res *= 2 586 | end 587 | 588 | @show to 589 | #show(to; allocations = false, sortby=:name) 590 | #show(to) 591 | print("\n") 592 | 593 | if show 594 | plt.pause(0.5) 595 | end 596 | end 597 | -------------------------------------------------------------------------------- /python/basic_gdip_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Test for Wrapper for GDIP library libGDIP.so 4 | 5 | ################################################## 6 | # Imports 7 | ################################################## 8 | 9 | import os, sys, math 10 | import numpy as np 11 | import matplotlib 12 | import matplotlib.pyplot as plt 13 | from matplotlib.patches import Circle, PathPatch 14 | 15 | # import the Python wrapper 16 | from dubinswrapper import DubinsWrapper as dubins 17 | 18 | ################################################## 19 | # Settings 20 | ################################################## 21 | 22 | # Turning radius 23 | turning_radius = 1.0 24 | # Step of the theta2 angle in the visualization 25 | step = 2 * math.pi / 100 26 | # Step for visualization 27 | step_size = 0.01 * turning_radius 28 | # Wait time for the animation 29 | wait_time = 0.01 30 | # Save figures into a directory "images" 31 | save_figures = False 32 | if len(sys.argv) > 1: 33 | if "-save" in sys.argv[1]: 34 | save_figures = True 35 | if save_figures: 36 | os.makedirs("images", exist_ok=True) 37 | # Sensing radius in case of GDIP solution 38 | sensing_radius_gdip = 0.5 39 | 40 | # End points 41 | point1 = [0.0, 0.0] 42 | point2 = [5.0, 0.0] 43 | 44 | # First angle interval (the second is changed) 45 | interval1 = [1.0, 0.5] 46 | 47 | # Enable only specific parts 48 | DUB = False 49 | DIP = False 50 | GDIP = True 51 | 52 | #matplotlib.rcParams['text.usetex'] = True 53 | 54 | ################################################## 55 | # Functions 56 | ################################################## 57 | 58 | def plot_points(points, specs = "b", zorder = 1): 59 | x_val = [x[0] for x in points] 60 | y_val = [x[1] for x in points] 61 | return plt.plot(x_val, y_val, specs, zorder = zorder) 62 | 63 | def plot_interval(point, interval, sensing_radius): 64 | ax = plt.gca() 65 | circle = Circle(point, sensing_radius, facecolor="yellow", edgecolor="orange", linewidth=1, alpha=0.2) 66 | ax.add_patch(circle) 67 | 68 | for angle in [interval[0], interval[0] + interval[1]]: 69 | last = point + turning_radius * np.array([math.cos(angle), math.sin(angle)]) 70 | points = [point, last] 71 | plot_points(points, specs = "y-", zorder = 1) 72 | 73 | def plot_arrow(px, py, angle, alpha = 1): 74 | rad = 0.7 * turning_radius 75 | dx = rad * math.cos(angle) 76 | dy = rad * math.sin(angle) 77 | plt.arrow(px, py, dx, dy, head_width=0.05, head_length=0.1, fc="k", ec="k", zorder=2, alpha=alpha) 78 | plt.plot([px, px + dx], [py, py + dy], "k", visible=False) 79 | 80 | def plot_figure(samples, point1, point2, title, sensing_radius = 0, interval1 = None, interval2 = None): 81 | # Clear and basic settings 82 | plt.clf() 83 | plt.axis("equal") 84 | plt.title(title) 85 | plt.xlabel("x [-]") 86 | plt.ylabel("y [-]") 87 | plt.ylim([-3, 3]) 88 | plt.xlim([-1.5, 6.5]) 89 | plt.grid(alpha=0.2) 90 | 91 | # Plot regions based on sensing radius 92 | if interval1 is not None: 93 | plot_interval(point1, interval1, sensing_radius) 94 | if interval2 is not None: 95 | plot_interval(point2, interval2, sensing_radius) 96 | 97 | # Plot samples of the Dubins maneuver 98 | ps = plot_points(samples) 99 | plt.legend([ps[0]], ["Dubins maneuver"]) 100 | 101 | # First arrow 102 | p1 = samples[0] 103 | p2 = samples[1] 104 | angle = math.atan2(p2[1]-p1[1], p2[0]-p1[0]) 105 | plot_arrow(p1[0], p1[1], angle) 106 | plot_arrow(point1[0], point1[1], angle, 0.3) 107 | plt.plot(p1[0], p1[1], "k.") 108 | 109 | # Second arrow 110 | p1 = samples[-1] 111 | p2 = samples[-2] 112 | angle = math.atan2(p1[1]-p2[1], p1[0]-p2[0]) 113 | plot_arrow(p1[0], p1[1], angle) 114 | plot_arrow(point2[0], point2[1], angle, 0.3) 115 | plt.plot(p1[0], p1[1], "k.") 116 | 117 | if not save_figures: 118 | plt.pause(wait_time) 119 | 120 | if save_figures: 121 | global image_idx 122 | filename = "images/{:05d}.png".format(image_idx) 123 | image_idx = image_idx + 1 124 | print(filename) 125 | plt.savefig(filename) 126 | 127 | image_idx = 0 128 | 129 | ################################################## 130 | # Basic Dubins maneuver 131 | ################################################## 132 | if DUB: 133 | for t2 in np.arange(0, 2*math.pi, step): 134 | start = [point1[0], point1[1], 1] 135 | final = [point2[0], point2[1], t2] 136 | dubins_path = dubins.shortest_path(start, final, turning_radius) 137 | samples, _ = dubins_path.sample_many(step_size) 138 | 139 | plot_figure(samples, point1, point2, "Dubins maneuver") 140 | 141 | ################################################## 142 | # Dubins Interval Problem (DIP) 143 | ################################################## 144 | if DIP: 145 | sensing_radius = 0.0 146 | 147 | for t2 in np.arange(0, 2*math.pi, step): 148 | interval2 = [2 + t2, 0.5] 149 | dubins_path = dubins.shortest_path_DIP(point1, interval1, point2, interval2, turning_radius) 150 | samples, _ = dubins_path.sample_many(step_size) 151 | 152 | plot_figure(samples, point1, point2, "Dubins Interval Problem (DIP)", sensing_radius, interval1, interval2) 153 | 154 | ################################################## 155 | # Generalized Dubins Interval Problem (GDIP) 156 | ################################################## 157 | if GDIP: 158 | sensing_radius = sensing_radius_gdip 159 | 160 | for t2 in np.arange(0, 2*math.pi, step): 161 | interval2 = (2 + t2, 0.5) 162 | step_size = 0.01 * turning_radius 163 | dubins_path = dubins.shortest_path_GDIP(point1, interval1, sensing_radius, point2, interval2, sensing_radius, turning_radius) 164 | samples, _ = dubins_path.sample_many(step_size) 165 | 166 | plot_figure(samples, point1, point2, "Generalized Dubins Interval Problem (GDIP)", sensing_radius, interval1, interval2) 167 | -------------------------------------------------------------------------------- /python/create_gif_example.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Basic GDIP example 4 | rm -rf images 5 | ./basic_gdip_example.py -save 6 | convert -delay 7 -loop 0 images/*.png basic-gdip-example.gif 7 | convert basic-gdip-example.gif -fuzz 5% -layers Optimize basic-gdip-example-small.gif 8 | 9 | # RSS'18 - DTRP example 10 | rm -rf images 11 | ./rss18_dtrp.py -save 12 | convert -delay 50 -loop 0 images/*.png rss-example.gif 13 | convert rss-example.gif -fuzz 5% -layers Optimize rss-example-small.gif 14 | -------------------------------------------------------------------------------- /python/dubinswrapper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os, sys, platform, math 4 | import ctypes as ct 5 | import numpy as np 6 | 7 | class DubinsWrapper: 8 | 9 | libgdip = None 10 | 11 | def init_library(self): 12 | try: 13 | file_extension = '.so' 14 | if platform.system() =='cli': 15 | file_extension = '.dll' 16 | elif platform.system() =='Windows': 17 | file_extension = '.dll' 18 | elif platform.system() == 'Darwin': 19 | file_extension = '.dylib' 20 | else: 21 | file_extension = '.so' 22 | 23 | libfullpath = os.path.abspath(os.path.join(os.path.dirname(__file__), '../gdip/lib/libGDIP' + file_extension)) 24 | print("Loading " + libfullpath) 25 | DubinsWrapper.libgdip = ct.CDLL(libfullpath) 26 | except: 27 | print ('----------------------------------------------------') 28 | print ('The GDIP library could not be loaded.') 29 | print ('----------------------------------------------------') 30 | exit(1) 31 | 32 | DubinsWrapper.libgdip.new_GdipAPI.restype = ct.c_void_p 33 | 34 | def __del__(self): 35 | #print("Destruct Dubins maneuver") 36 | self.gdip_destruct(self.object_hanle) 37 | 38 | def __init__(self): 39 | 40 | #initialize the GDIP library (only for the first time) 41 | if DubinsWrapper.libgdip is None: 42 | self.init_library() 43 | 44 | # create instance of the maneuver 45 | self.object_hanle = self.libgdip.new_GdipAPI() 46 | 47 | self.gdip_set_configurations = ct.CFUNCTYPE \ 48 | (None, ct.c_void_p, ct.c_double*3, ct.c_double*3, ct.c_double) \ 49 | (("python_set_configurations", DubinsWrapper.libgdip)) 50 | 51 | self.gdip_set_configurations_dip = ct.CFUNCTYPE \ 52 | (None, ct.c_void_p, ct.c_double*2, ct.c_double*2, ct.c_double*2, ct.c_double*2, ct.c_double) \ 53 | (("python_set_configurations_dip", DubinsWrapper.libgdip)) 54 | 55 | self.gdip_set_configurations_gdip = ct.CFUNCTYPE \ 56 | (None, ct.c_void_p, ct.c_double*2, ct.c_double*2, ct.c_double, ct.c_double*2, ct.c_double*2, ct.c_double, ct.c_double) \ 57 | (("python_set_configurations_gdip", DubinsWrapper.libgdip)) 58 | 59 | self.gdip_get_length = ct.CFUNCTYPE (ct.c_double, ct.c_void_p) (("python_get_length", DubinsWrapper.libgdip)) 60 | 61 | self.gdip_sample_state_to_tmp = ct.CFUNCTYPE (None, ct.c_void_p, ct.c_double) (("python_sample_state_to_tmp", DubinsWrapper.libgdip)) 62 | 63 | self.gdip_get_tmp_x = ct.CFUNCTYPE(ct.c_double, ct.c_void_p) (("python_get_tmp_x", DubinsWrapper.libgdip)) 64 | self.gdip_get_tmp_y = ct.CFUNCTYPE(ct.c_double, ct.c_void_p) (("python_get_tmp_y", DubinsWrapper.libgdip)) 65 | self.gdip_get_tmp_theta = ct.CFUNCTYPE(ct.c_double, ct.c_void_p) (("python_get_tmp_theta", DubinsWrapper.libgdip)) 66 | 67 | self.gdip_destruct = ct.CFUNCTYPE(None, ct.c_void_p) (("python_destruct", DubinsWrapper.libgdip)) 68 | 69 | @staticmethod 70 | def shortest_path(start, end, turning_radius): 71 | """ 72 | Construct Dubins maneuver between two configurations 73 | 74 | Parameters 75 | ---------- 76 | start: numpy array(double*3) 77 | start configuration 78 | end: numpy array(double*3) 79 | end configuration 80 | turning_radius: double 81 | minimum turning radius 82 | """ 83 | man = DubinsWrapper() 84 | 85 | arrtype = ct.c_double * 3 86 | arr_p1 = arrtype() 87 | arr_p2 = arrtype() 88 | c_radius = ct.c_double() 89 | for i in range(0,3): 90 | arr_p1[i] = start[i] 91 | arr_p2[i] = end[i] 92 | c_radius = turning_radius 93 | 94 | man.gdip_set_configurations(man.object_hanle, arr_p1, arr_p2, c_radius) 95 | return man 96 | 97 | @staticmethod 98 | def shortest_path_DIP(point1, interval1, point2, interval2, turning_radius): 99 | """ 100 | Construct Dubins maneuver between two configurations 101 | 102 | Parameters 103 | ---------- 104 | point1: numpy array(double*2) 105 | start position 106 | interval1: numpy array(double*2) 107 | angle interval for point1 (right_angle, diff) 108 | the interval is then [right_angle, right_angle + diff] 109 | point2: numpy array(double*2) 110 | end position 111 | interval2: numpy array(double*2) 112 | angle interval for point2 (right_angle, diff) 113 | the interval is then [right_angle, right_angle + diff] 114 | turning_radius: double 115 | minimum turning radius 116 | """ 117 | man = DubinsWrapper() 118 | 119 | arrtype = ct.c_double * 2 120 | arr_p1 = arrtype() 121 | arr_i1 = arrtype() 122 | arr_p2 = arrtype() 123 | arr_i2 = arrtype() 124 | c_radius = ct.c_double() 125 | 126 | arr_p1[0] = point1[0] 127 | arr_p1[1] = point1[1] 128 | 129 | arr_i1[0] = interval1[0] 130 | arr_i1[1] = interval1[1] 131 | 132 | arr_p2[0] = point2[0] 133 | arr_p2[1] = point2[1] 134 | 135 | arr_i2[0] = interval2[0] 136 | arr_i2[1] = interval2[1] 137 | 138 | c_radius = turning_radius 139 | 140 | man.gdip_set_configurations_dip(man.object_hanle, arr_p1, arr_i1, arr_p2, arr_i2, c_radius) 141 | return man 142 | 143 | @staticmethod 144 | def shortest_path_GDIP(point1, interval1, radius1, point2, interval2, radius2, turning_radius): 145 | """ 146 | Construct Dubins maneuver between two configurations 147 | 148 | Parameters 149 | ---------- 150 | point1: numpy array(double*2) 151 | start position 152 | interval1: numpy array(double*2) 153 | angle interval for point1 (right_angle, diff) 154 | the interval is then [right_angle, right_angle + diff] 155 | point2: numpy array(double*2) 156 | end position 157 | interval2: numpy array(double*2) 158 | angle interval for point2 (right_angle, diff) 159 | the interval is then [right_angle, right_angle + diff] 160 | turning_radius: double 161 | minimum turning radius 162 | """ 163 | man = DubinsWrapper() 164 | 165 | arrtype = ct.c_double * 2 166 | arr_p1 = arrtype() 167 | arr_i1 = arrtype() 168 | arr_p2 = arrtype() 169 | arr_i2 = arrtype() 170 | 171 | arr_p1[0] = point1[0] 172 | arr_p1[1] = point1[1] 173 | 174 | arr_i1[0] = interval1[0] 175 | arr_i1[1] = interval1[1] 176 | 177 | arr_p2[0] = point2[0] 178 | arr_p2[1] = point2[1] 179 | 180 | arr_i2[0] = interval2[0] 181 | arr_i2[1] = interval2[1] 182 | 183 | c_radius = ct.c_double() 184 | c_radius = turning_radius 185 | 186 | c_radius1 = ct.c_double() 187 | c_radius1 = radius1 188 | 189 | c_radius2 = ct.c_double() 190 | c_radius2 = radius2 191 | 192 | man.gdip_set_configurations_gdip(man.object_hanle, arr_p1, arr_i1, c_radius1, arr_p2, arr_i2, c_radius2, c_radius) 193 | return man 194 | 195 | def get_length(self): 196 | """ 197 | Get length of the maneuver 198 | 199 | Returns 200 | ------- 201 | bool 202 | True if there is collision, False otherwise 203 | """ 204 | return self.gdip_get_length(self.object_hanle) 205 | 206 | def sample_many(self, step): 207 | """ 208 | Sample the manuver based on the step 209 | 210 | Parameters 211 | ---------- 212 | step: double 213 | step for the sampling 214 | 215 | Returns 216 | ------- 217 | states 218 | """ 219 | 220 | length = self.get_length() 221 | lens = np.arange(0, length, step) 222 | path = [] 223 | 224 | for l in lens: 225 | self.gdip_sample_state_to_tmp(self.object_hanle, l) 226 | state = [self.gdip_get_tmp_x(self.object_hanle), self.gdip_get_tmp_y(self.object_hanle), self.gdip_get_tmp_theta(self.object_hanle)] 227 | path.append(state) 228 | 229 | return [path, lens] 230 | 231 | if __name__ == "__main__": 232 | a = DubinsWrapper() 233 | a.shortest_path((0,0,0), (1,1,2), 1) 234 | print(a.get_length()) -------------------------------------------------------------------------------- /python/problems/gdip-n10.txt: -------------------------------------------------------------------------------- 1 | 1 6.71627 8.3767 2 | 2 6.40217 9.49307 3 | 3 3.41556 10.2848 4 | 4 5.91928 7.20563 5 | 5 3.90768 3.7961 6 | 6 7.52977 3.70221 7 | 7 8.84408 3.80583 8 | 8 10.6659 6.6348 9 | 9 12.1318 7.59114 10 | 10 10.5947 10.1087 11 | -------------------------------------------------------------------------------- /python/rss18_dtrp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys, os, re, math, copy 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | from matplotlib.patches import Circle, PathPatch 7 | 8 | # import the Python wrapper 9 | from dubinswrapper import DubinsWrapper as dubins 10 | 11 | ################################################ 12 | # Scenarios 13 | ################################################ 14 | #define planning problems: 15 | # map file 16 | # minimum turning radius 17 | # sensing radius 18 | # solver type 19 | scenarios = [ 20 | ("./problems/gdip-n10.txt", 1, 1, 'DTSPN-GDIP'), 21 | #("./problems/gdip-n10.txt", 0.5, 1, 'DTSPN-GDIP'), 22 | #("./problems/gdip-n10.txt", 1, 0.5, 'DTSPN-GDIP'), 23 | ] 24 | 25 | ################################################ 26 | # Settings 27 | ################################################ 28 | 29 | visualize = True 30 | show = True 31 | 32 | # Save figures into a directory "images" 33 | save_figures = False 34 | if len(sys.argv) > 1: 35 | if "-save" in sys.argv[1]: 36 | visualize = True 37 | save_figures = True 38 | if save_figures: 39 | os.makedirs("images", exist_ok=True) 40 | 41 | ################################################## 42 | # Functions 43 | ################################################## 44 | 45 | def load_map(filename): 46 | """Read config with goal positions""" 47 | goals = [] 48 | with open(filename) as fp: 49 | for line in fp: 50 | label, x, y = line.split() 51 | goals.append((float(x), float(y))) 52 | return goals 53 | 54 | def plot_points(points, specs = 'b'): 55 | x_val = [x[0] for x in points] 56 | y_val = [x[1] for x in points] 57 | plt.plot(x_val, y_val, specs) 58 | 59 | def plot_circle(xy, radius): 60 | ax = plt.gca() 61 | circle = Circle(xy, radius, facecolor='yellow',edgecolor="orange", linewidth=1, alpha=0.2) 62 | ax.add_patch(circle) 63 | 64 | def dist_euclidean(coord1, coord2): 65 | (x1, y1) = coord1 66 | (x2, y2) = coord2 67 | (dx, dy) = (x2 - x1, y2 - y1) 68 | return math.sqrt(dx * dx + dy * dy) 69 | 70 | # cache results from computing length of the GDIP 71 | lowerPathGDIPLenCache = {} 72 | 73 | def lowerPathGDIP(s1, s2, turning_radius, cached = False): 74 | """Compute lower-bound path using GDIP between two configurations 75 | 76 | Arguments: 77 | s1 - start; s2 - end; turning_radius 78 | cached - compute only distance and cache the results 79 | Returns: 80 | Dubins maneuver (DubinsWrapper) 81 | """ 82 | if cached: 83 | key = (s1, s2) 84 | if key in lowerPathGDIPLenCache: 85 | length = lowerPathGDIPLenCache[key] 86 | return (None, length) 87 | 88 | interval1 = [s1.alpha1, s1.alpha2 - s1.alpha1] 89 | interval2 = [s2.alpha1, s2.alpha2 - s2.alpha1] 90 | path = dubins.shortest_path_GDIP(s1.center, interval1, s1.radius, s2.center, interval2, s2.radius, turning_radius) 91 | length = path.get_length() 92 | 93 | if cached: 94 | lowerPathGDIPLenCache[key] = length 95 | return (None, length) 96 | else: 97 | return (path, length) 98 | 99 | def upperPathGDIP(s1, s2, turning_radius, cached = False): 100 | """Compute feasible Dubins path two configurations 101 | 102 | Arguments: 103 | s1 - start; s2 - end; turning_radius 104 | Returns: 105 | (Dubins maneuver 'DubinsWrapper', length) 106 | """ 107 | q1 = s1.getFeasibleState() 108 | q2 = s2.getFeasibleState() 109 | dubins_path = dubins.shortest_path(q1, q2, turning_radius) 110 | return (dubins_path, dubins_path.get_length()) 111 | 112 | def compute_distances(samples, dst_fce, turning_radius = 0): 113 | n = len(samples) 114 | distances = [] 115 | 116 | for i in range(n): 117 | ss1 = samples[i] 118 | ss2 = samples[(i+1) % n] 119 | 120 | n1 = len(ss1) 121 | n2 = len(ss2) 122 | 123 | sh = np.full((n1, n2), np.inf) 124 | for i1 in range(n1): 125 | for i2 in range(n2): 126 | dist = dst_fce(ss1[i1], ss2[i2], turning_radius, cached=True) 127 | sh[i1][i2] = dist[1] 128 | distances.append(sh) 129 | 130 | return distances 131 | 132 | def find_shortest_tour(distances): 133 | n = len(distances) 134 | best_len = math.inf 135 | best_tour = [] 136 | 137 | # maximal number of samples 138 | k_max = max([len(x) for x in distances]) 139 | 140 | no_start = len(distances[0]) 141 | for start in range(no_start): 142 | # shortest sh[region_idx][sample_idx] 143 | # contains (prev, length) 144 | sh = np.full((n+1, k_max), np.inf) 145 | # used edge 146 | prev = np.full((n+1, k_max), -1) 147 | 148 | sh[0,start] = 0 149 | 150 | for region_idx in range(n): 151 | n1 = len(distances[region_idx]) 152 | n2 = len(distances[region_idx][0]) 153 | for idx2 in range(n2): 154 | dst = sh[region_idx][:n1] + distances[region_idx][:,idx2] 155 | sh[region_idx+1][idx2] = np.min(dst) 156 | prev[region_idx+1][idx2]= np.argmin(dst) 157 | 158 | act_sol = sh[-1][start] 159 | if act_sol < best_len: 160 | best_len = act_sol 161 | tour = [] 162 | act = start 163 | for i in range(n): 164 | act = prev[n-i][act] 165 | tour.append(act) 166 | best_tour = list(reversed(tour)) 167 | 168 | return best_tour 169 | 170 | def retrieve_path(samples, dst_fce, turning_radius, selected_samples): 171 | n = len(samples) 172 | path = [] 173 | for a in range(0,n): 174 | g1 = samples[a][selected_samples[a]] 175 | g2 = samples[(a+1) % n][selected_samples[(a+1) % n]] 176 | path.append(dst_fce(g1, g2, turning_radius)) 177 | return path 178 | 179 | def path_len(path): 180 | return sum_list([dub.get_length() for dub in path]) 181 | 182 | def plot_path(path, turning_radius, settings): 183 | step_size = 0.01 * turning_radius 184 | for dub in path: 185 | configurations, _ = dub.sample_many(step_size) 186 | plot_points(configurations, settings) 187 | 188 | ################################################## 189 | # Target region given by the location and its sensing radius 190 | ################################################## 191 | class TargetRegion: 192 | def __init__(self, center, radius): 193 | self.center = center 194 | self.radius = radius 195 | 196 | def get_position_at_boundary(self, beta): 197 | return self.center + self.radius * np.array([math.cos(beta), math.sin(beta)]) 198 | 199 | ################################################## 200 | # Sample on the given target region 201 | ################################################## 202 | class Sample: 203 | def __init__(self, targetRegion): 204 | # reference to the specific target region of the sample 205 | self.target = targetRegion 206 | 207 | # heading angle interval 208 | self.alpha1 = 0 209 | self.alpha2 = 2 * math.pi 210 | self.alphaResolution = 1 211 | # position (on the boundary) interval 212 | self.beta1 = 0 213 | self.beta2 = 2 * math.pi 214 | self.betaResolution = 1 215 | 216 | # center and radius of the position neighborhood on the boundary of the target region 217 | self.center = np.array(targetRegion.center) 218 | self.radius = targetRegion.radius 219 | 220 | def split(self, resolution): 221 | """Split the actual sample into two new ones. 222 | The first is stored directly in the actual sample, and the second is returned. 223 | If the required resolution is already met, then nothing is done and None is returned. 224 | 225 | Parameters: 226 | resolution: the requred resolution 227 | Returns: 228 | Sample - the second new sample 229 | """ 230 | # prefer splitting according position resolution 231 | if self.betaResolution < resolution: 232 | sam1 = copy.copy(self) 233 | sam2 = copy.copy(self) 234 | sam1.betaResolution = sam2.betaResolution = 2 * self.betaResolution 235 | sam1.beta2 = sam2.beta1 = (self.beta1 + self.beta2) / 2 236 | sam1.update_center_radius() 237 | sam2.update_center_radius() 238 | return [sam1, sam2] 239 | if self.alphaResolution < resolution: 240 | sam1 = copy.copy(self) 241 | sam2 = copy.copy(self) 242 | sam1.alphaResolution = sam2.alphaResolution = 2 * self.alphaResolution 243 | sam1.alpha2 = sam2.alpha1 = (self.alpha1 + self.alpha2) / 2 244 | return [sam1, sam2] 245 | return None 246 | 247 | def update_center_radius(self): 248 | p1 = self.target.get_position_at_boundary(self.beta1) 249 | p2 = self.target.get_position_at_boundary(self.beta2) 250 | self.center = (p1 + p2) / 2 251 | self.radius = dist_euclidean(p1, p2) / 2 252 | 253 | def getFeasibleState(self): 254 | pos = self.target.get_position_at_boundary(self.beta1) 255 | q = np.zeros(3) 256 | q[0:2] = pos 257 | q[2] = self.alpha1 258 | return q 259 | 260 | def plot(self): 261 | ax = plt.gca() 262 | circle = Circle(self.center, self.radius, facecolor=None ,edgecolor="green", linewidth=1, alpha=0.2) 263 | ax.add_patch(circle) 264 | 265 | ################################################## 266 | # Sampling structure which holds all the used samples 267 | ################################################## 268 | class Sampling: 269 | def __init__(self, centers, sensingRadius): 270 | self.targets = [TargetRegion(c, sensingRadius) for c in centers] 271 | self.samples = [[Sample(t)] for t in self.targets] 272 | 273 | def refine_samples(self, selected, resolution): 274 | """Refine the seleted samples if the required resolution is not met. 275 | 276 | Parameters: 277 | slected: indexes of the selected samples (vector 1 x n) 278 | resolution: the requred resolution 279 | Returns: 280 | boolean - true if any sample is refined 281 | """ 282 | n = len(self.samples) 283 | refined = False 284 | for i in range(n): 285 | to_split = selected[i] 286 | samp = self.samples[i][to_split] 287 | res = samp.split(resolution) 288 | if not res is None: 289 | self.samples[i][to_split] = res[0] 290 | self.samples[i].append(res[1]) 291 | refined = True 292 | return refined 293 | 294 | ################################################## 295 | # The main solver class 296 | ################################################## 297 | class GDIPSolver: 298 | def __init__(self, turning_radius, goals, sensing_radius): 299 | self.turning_radius = turning_radius 300 | self.sensing_radius = sensing_radius 301 | self.goals = goals 302 | self.sampling = Sampling(goals, sensing_radius) 303 | 304 | self.lower_path = [] 305 | self.upper_path = [] 306 | 307 | self.lower_bound = 0 308 | self.upper_bound = math.inf 309 | 310 | def plot_map(self): 311 | plt.clf() 312 | plt.axis('equal') 313 | plot_points(self.goals, 'ro') 314 | if self.sensing_radius != None: 315 | for goal in self.goals: 316 | plot_circle(goal, self.sensing_radius) 317 | 318 | def plot_tour_and_return_length(self, selected_samples, maneuver_function, color): 319 | sampling = self.sampling 320 | n = len(self.sampling.samples) 321 | step_size = 0.01 * self.turning_radius 322 | length = 0 323 | for a in range(0,n): 324 | g1 = sampling.samples[a][selected_samples[a]] 325 | g2 = sampling.samples[(a+1) % n][selected_samples[(a+1) % n]] 326 | 327 | path = maneuver_function(g1, g2, self.turning_radius) 328 | length += path[1] 329 | configurations, _ = path[0].sample_many(step_size) 330 | if visualize: 331 | plot_points(configurations, color) 332 | return length 333 | 334 | def plot_actual_and_return_bounds(self): 335 | """Plot the actual sampling, lower and upper bound path 336 | 337 | Returns: 338 | (double, double) - lower bound, upper bound 339 | """ 340 | if visualize: 341 | self.plot_map() 342 | 343 | for s in self.sampling.samples: 344 | for ss in s: 345 | ss.plot() 346 | 347 | lower_selected_samples = self.find_lower_bound_tour() 348 | upper_selected_samples = self.find_upper_bound_tour() 349 | 350 | lower_bound = self.plot_tour_and_return_length(lower_selected_samples, lowerPathGDIP, 'r-') 351 | upper_bound = self.plot_tour_and_return_length(upper_selected_samples, upperPathGDIP, 'b-') 352 | return (lower_bound, upper_bound) 353 | 354 | def find_lower_bound_tour(self): 355 | """Select the samples which represent the shortest lower bound tour 356 | 357 | Returns: 358 | indexes of the samples (vector 1 x n) 359 | """ 360 | distances = compute_distances(self.sampling.samples, lowerPathGDIP, turning_radius = self.turning_radius) 361 | selected_samples = find_shortest_tour(distances) 362 | return selected_samples 363 | 364 | def find_upper_bound_tour(self): 365 | """Select the samples which represent the shortest upper bound (feasible) tour 366 | 367 | Returns: 368 | indexes of the samples (vector 1 x n) 369 | """ 370 | distances = compute_distances(self.sampling.samples, upperPathGDIP, turning_radius = self.turning_radius) 371 | selected_samples = find_shortest_tour(distances) 372 | return selected_samples 373 | 374 | 375 | ################################################## 376 | # Main loop over selected scenarios 377 | ################################################## 378 | for scenario in scenarios: 379 | # Load the problem and scenario settings 380 | filename = scenario[0] 381 | goals = load_map(filename) 382 | turning_radius = scenario[1] 383 | sensing_radius = scenario[2] 384 | solver_type = scenario[3] 385 | 386 | #tour planning part 387 | solver = GDIPSolver(turning_radius, goals, sensing_radius) 388 | solver.plot_actual_and_return_bounds() 389 | 390 | print("\n--- Problem: {} Turning radius: {:6.2f} Sensing radius: {:6.2f} ---" 391 | .format(filename, turning_radius, sensing_radius)) 392 | 393 | if show: 394 | plt.pause(0.1) 395 | 396 | max_resolution = 64 397 | act_res = 4 398 | while act_res <= max_resolution: 399 | refined = True 400 | while refined: 401 | selected_samples = solver.find_lower_bound_tour() 402 | refined = solver.sampling.refine_samples(selected_samples, act_res) 403 | (lower_bound, upper_bound) = solver.plot_actual_and_return_bounds() 404 | gap = (upper_bound - lower_bound) / upper_bound * 100.0 405 | print("Res: {:4d} Lower: {:6.2f} Upper: {:6.2f} Gap(%): {:6.2f}" 406 | .format(act_res, lower_bound, upper_bound, gap)) 407 | 408 | if visualize: 409 | plt.title("Maximum resolution: {:4d}".format(act_res)) 410 | if show: 411 | plt.pause(0.1) 412 | if save_figures: 413 | plt.savefig("images/dtrp-res-{:04d}.png".format(act_res)) 414 | 415 | act_res *= 2 416 | 417 | if show: 418 | plt.pause(2) 419 | 420 | --------------------------------------------------------------------------------