├── .gitattributes ├── doc └── figures │ ├── splines.gif │ ├── color_codes.png │ ├── num_angles.png │ ├── planner_gui.png │ ├── trav_map_1.png │ ├── gazebo_teleop.png │ ├── max_curve_length.png │ ├── splines_filtered.gif │ ├── ugv_nav4d_logo.jpeg │ ├── max_curve_length_2.png │ ├── max_curve_length_3.png │ ├── planner_gui_result.png │ ├── motion_primitives_gui.png │ ├── set_goal_pose_rviz2.png │ ├── mls_visualization_rviz2.png │ ├── mls_visualization_rviz2_2.png │ ├── mls_visualization_rviz2_3.png │ └── mls_visualization_rviz2_4.png ├── paper ├── figures │ ├── debug_gui.png │ ├── trav_map.png │ ├── color_codes.png │ ├── parking_deck.png │ ├── motion_primitives.png │ ├── space_hall_crater.png │ ├── trav_map_space_hall.png │ ├── motion_planning_hunter.png │ ├── trav_map_and_space_hall.png │ └── test_environment │ │ ├── multi_surface_trav_map.png │ │ ├── multi_surface_test_mls_map.png │ │ └── multi_surface_planned_trajectory.png ├── paper.bib └── paper.md ├── .gitignore ├── src ├── DiscreteTheta.cpp ├── ugv_nav4d.pc.in ├── gui │ ├── ugv_nav4d_gui.pc.in │ ├── ugv_nav4d_gui-qt5.pc.in │ ├── Main.cpp │ ├── ReplayDump.cpp │ ├── CMakeLists.txt │ ├── PlannerGui.h │ └── PlannerGui.cpp ├── Logger.hpp ├── test │ ├── CMakeLists.txt │ ├── test_DiscreteTheta.cpp │ ├── test_PreComputedMotions.cpp │ ├── test_EnvironmentXYZTheta.cpp │ └── test_Planner.cpp ├── Dijkstra.hpp ├── PlannerConfig.hpp ├── DebugDrawingDeclarations.cpp ├── CMakeLists.txt ├── PlannerDump.hpp ├── Dijkstra.cpp ├── DiscreteTheta.hpp ├── PathStatistic.hpp ├── Mobility.hpp ├── PreComputedMotions.hpp ├── PlannerDump.cpp ├── Planner.hpp ├── Planner.cpp ├── EnvironmentXYZTheta.hpp ├── PathStatistic.cpp └── PreComputedMotions.cpp ├── .gitlab-ci.yml ├── source_dependencies ├── env.sh.in ├── install_os_dependencies.bash ├── build.bash └── CMakeLists.txt ├── .github └── workflows │ ├── c-cpp.yml │ └── docs.yml ├── manifest.xml ├── CITATION.cff ├── LICENSE ├── CMakeLists.txt └── CONTRIBUTING.md /.gitattributes: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /doc/figures/splines.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/splines.gif -------------------------------------------------------------------------------- /doc/figures/color_codes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/color_codes.png -------------------------------------------------------------------------------- /doc/figures/num_angles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/num_angles.png -------------------------------------------------------------------------------- /doc/figures/planner_gui.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/planner_gui.png -------------------------------------------------------------------------------- /doc/figures/trav_map_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/trav_map_1.png -------------------------------------------------------------------------------- /paper/figures/debug_gui.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/debug_gui.png -------------------------------------------------------------------------------- /paper/figures/trav_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/trav_map.png -------------------------------------------------------------------------------- /doc/figures/gazebo_teleop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/gazebo_teleop.png -------------------------------------------------------------------------------- /paper/figures/color_codes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/color_codes.png -------------------------------------------------------------------------------- /doc/figures/max_curve_length.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/max_curve_length.png -------------------------------------------------------------------------------- /doc/figures/splines_filtered.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/splines_filtered.gif -------------------------------------------------------------------------------- /doc/figures/ugv_nav4d_logo.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/ugv_nav4d_logo.jpeg -------------------------------------------------------------------------------- /paper/figures/parking_deck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/parking_deck.png -------------------------------------------------------------------------------- /doc/figures/max_curve_length_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/max_curve_length_2.png -------------------------------------------------------------------------------- /doc/figures/max_curve_length_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/max_curve_length_3.png -------------------------------------------------------------------------------- /doc/figures/planner_gui_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/planner_gui_result.png -------------------------------------------------------------------------------- /doc/figures/motion_primitives_gui.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/motion_primitives_gui.png -------------------------------------------------------------------------------- /doc/figures/set_goal_pose_rviz2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/set_goal_pose_rviz2.png -------------------------------------------------------------------------------- /paper/figures/motion_primitives.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/motion_primitives.png -------------------------------------------------------------------------------- /paper/figures/space_hall_crater.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/space_hall_crater.png -------------------------------------------------------------------------------- /paper/figures/trav_map_space_hall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/trav_map_space_hall.png -------------------------------------------------------------------------------- /doc/figures/mls_visualization_rviz2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/mls_visualization_rviz2.png -------------------------------------------------------------------------------- /doc/figures/mls_visualization_rviz2_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/mls_visualization_rviz2_2.png -------------------------------------------------------------------------------- /doc/figures/mls_visualization_rviz2_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/mls_visualization_rviz2_3.png -------------------------------------------------------------------------------- /doc/figures/mls_visualization_rviz2_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/doc/figures/mls_visualization_rviz2_4.png -------------------------------------------------------------------------------- /paper/figures/motion_planning_hunter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/motion_planning_hunter.png -------------------------------------------------------------------------------- /paper/figures/trav_map_and_space_hall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/trav_map_and_space_hall.png -------------------------------------------------------------------------------- /paper/figures/test_environment/multi_surface_trav_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/test_environment/multi_surface_trav_map.png -------------------------------------------------------------------------------- /paper/figures/test_environment/multi_surface_test_mls_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/test_environment/multi_surface_test_mls_map.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *.sw? 3 | .kdev4 4 | .idea 5 | *.kdev4 6 | *.project 7 | *.cproject 8 | .env 9 | */install/* 10 | source_dependencies/env.sh 11 | *.bin 12 | *.ply 13 | -------------------------------------------------------------------------------- /paper/figures/test_environment/multi_surface_planned_trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dfki-ric/ugv_nav4d/HEAD/paper/figures/test_environment/multi_surface_planned_trajectory.png -------------------------------------------------------------------------------- /src/DiscreteTheta.cpp: -------------------------------------------------------------------------------- 1 | #include "DiscreteTheta.hpp" 2 | 3 | std::ostream& operator<< (std::ostream& stream, const DiscreteTheta& angle) 4 | { 5 | stream << angle.getTheta(); 6 | return stream; 7 | } -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | include: 2 | - project: 'build_server/gitlab_ci_includes' 3 | file: 'software_catalogue.yml' 4 | 5 | - project: 'sw-backbone/continuous-integration/ric-backbone-buildconf' 6 | file: 'gitlab-ci-include.yml' 7 | 8 | -------------------------------------------------------------------------------- /source_dependencies/env.sh.in: -------------------------------------------------------------------------------- 1 | export CMAKE_PREFIX_PATH=@CMAKE_INSTALL_PREFIX@/share/rock/cmake/:$CMAKE_PREFIX_PATH 2 | export PKG_CONFIG_PATH=@CMAKE_INSTALL_PREFIX@/lib/pkgconfig/:$PKG_CONFIG_PATH 3 | export LD_LIBRARY_PATH=@CMAKE_INSTALL_PREFIX@/lib:@CMAKE_INSTALL_PREFIX@/lib64:$LD_LIBRARY_PATH 4 | export PATH=@CMAKE_INSTALL_PREFIX@/bin:$PATH 5 | export VIZKIT_PLUGIN_RUBY_PATH=@CMAKE_INSTALL_PREFIX@/lib -------------------------------------------------------------------------------- /src/ugv_nav4d.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=@CMAKE_INSTALL_PREFIX@ 3 | libdir=${prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: @TARGET_NAME@ 7 | Description: @PROJECT_DESCRIPTION@ 8 | Version: @PROJECT_VERSION@ 9 | Requires: @PKGCONFIG_REQUIRES@ 10 | Libs: -L${libdir} -l@TARGET_NAME@ @PKGCONFIG_LIBS@ 11 | Cflags: -I${includedir} @PKGCONFIG_CFLAGS@ 12 | 13 | -------------------------------------------------------------------------------- /src/gui/ugv_nav4d_gui.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=@CMAKE_INSTALL_PREFIX@ 3 | libdir=${prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: @TARGET_NAME@ 7 | Description: @PROJECT_DESCRIPTION@ 8 | Version: @PROJECT_VERSION@ 9 | Requires: @PKGCONFIG_REQUIRES@ 10 | Libs: -L${libdir} -l@TARGET_NAME@ @PKGCONFIG_LIBS@ 11 | Cflags: -I${includedir} @PKGCONFIG_CFLAGS@ 12 | 13 | -------------------------------------------------------------------------------- /src/gui/ugv_nav4d_gui-qt5.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=@CMAKE_INSTALL_PREFIX@ 3 | libdir=${prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: @TARGET_NAME@ 7 | Description: @PROJECT_DESCRIPTION@ 8 | Version: @PROJECT_VERSION@ 9 | Requires: @PKGCONFIG_REQUIRES@ 10 | Libs: -L${libdir} -l@TARGET_NAME@ @PKGCONFIG_LIBS@ 11 | Cflags: -I${includedir} @PKGCONFIG_CFLAGS@ 12 | 13 | -------------------------------------------------------------------------------- /.github/workflows/c-cpp.yml: -------------------------------------------------------------------------------- 1 | name: C/C++ CI 2 | 3 | on: 4 | push: 5 | branches: [ "main" ] 6 | pull_request: 7 | branches: [ "main" ] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | steps: 13 | - uses: actions/checkout@v4 14 | - name: build_ugv_nav4d 15 | run: | 16 | mkdir build && cd build 17 | cmake -DINSTALL_DEPS=ON -DCMAKE_INSTALL_PREFIX=./install .. 18 | make -j install 19 | -------------------------------------------------------------------------------- /source_dependencies/install_os_dependencies.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS="doxygen xdot qttools5-dev qttools5-dev-tools libopenscenegraph-dev libeigen3-dev libcgal-dev libpcl-dev libboost-filesystem-dev libboost-serialization-dev libboost-system-dev" 4 | 5 | if dpkg --verify $DEPS; then 6 | echo "All OS dependencies are installed already. To update them run:" 7 | echo "# apt-get update && apt-get upgrade" 8 | else 9 | sudo apt-get update 10 | sudo apt-get install -y $DEPS 11 | fi 12 | 13 | -------------------------------------------------------------------------------- /src/Logger.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LOG_PLAN_H 2 | #define LOG_PLAN_H 3 | #define LOG_PLAN(...) PlanTrace(__FILE__, __FUNCTION__, __LINE__, __VA_ARGS__) 4 | #include 5 | 6 | 7 | 8 | void privTrace() { 9 | std::cout << std::endl; 10 | } 11 | template 12 | void privTrace(T t, Args... args) 13 | { 14 | std::cout << t << " "; 15 | privTrace(args...); 16 | } 17 | template 18 | void PlanTrace(const char* file, const char* func, const int line, Args&& ...args) 19 | { 20 | std::cout << '[' << file << '|' << func << '@' << line << "]:\t"; 21 | privTrace(args...); 22 | } 23 | #endif 24 | -------------------------------------------------------------------------------- /src/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Boost REQUIRED COMPONENTS filesystem serialization) 2 | 3 | rock_executable(test_Planner 4 | SOURCES test_Planner.cpp 5 | DEPS Boost::filesystem 6 | DEPS_PKGCONFIG ugv_nav4d 7 | ) 8 | 9 | rock_executable(test_EnvironmentXYZTheta 10 | SOURCES test_EnvironmentXYZTheta.cpp 11 | DEPS Boost::filesystem Boost::serialization 12 | DEPS_PKGCONFIG ugv_nav4d 13 | ) 14 | 15 | rock_executable(test_DiscreteTheta 16 | SOURCES test_DiscreteTheta.cpp 17 | DEPS_PKGCONFIG ugv_nav4d 18 | ) 19 | 20 | rock_executable(test_PreComputedMotions 21 | SOURCES test_PreComputedMotions.cpp 22 | DEPS Boost::filesystem 23 | DEPS_PKGCONFIG ugv_nav4d 24 | ) 25 | -------------------------------------------------------------------------------- /src/Dijkstra.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace maps { namespace grid 6 | { 7 | class TraversabilityNodeBase; 8 | }} 9 | 10 | namespace traversability_generator3d{ 11 | class TraversabilityConfig; 12 | } 13 | 14 | namespace ugv_nav4d 15 | { 16 | class Dijkstra 17 | { 18 | public: 19 | 20 | /** Computes the heuristic cost from @p source to all reachable nodes 21 | * @param outDistances mapping from node to distance 22 | * @param maxDist the maximum possible distance*/ 23 | static void computeCost(const maps::grid::TraversabilityNodeBase* source, 24 | std::unordered_map &outDistances, 25 | const traversability_generator3d::TraversabilityConfig& config); 26 | }; 27 | 28 | } -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Janosch Machowinski/Janosch.Machowinski@dfki.de 5 | Arne Böckmann/arne.boeckmann@dfki.de 6 | mulo01@dfki.de 7 | 1 8 | BSD 3-Clause 9 | https://github.com/dfki-ric/ugv_nav4d 10 | http:// 11 | 12 | Planning 13 | motion primitives 14 | sampling based motion planner 15 | Multi level surface planner 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | needs_opt 24 | 25 | -------------------------------------------------------------------------------- /source_dependencies/build.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ "$#" -ne 1 ]; then 4 | echo 5 | echo "Please specify a prefix. If you want system wide installation use '/usr/local' as prefix. for a local install use e.g. './install'" 6 | echo "In case you use a local prefix, please source the generated env.sh script in this folder to set up the paths before building the main library" 7 | echo 8 | exit -1 9 | fi 10 | 11 | if [[ $1 != .* && $1 != /* ]]; then 12 | echo 13 | echo -e "\033[1;33mPlease specify a global path or a local path with '/' or './' \033[1;0m" 14 | echo 15 | exit -1 16 | fi 17 | 18 | #download and build (in subshell to return to original folder) 19 | 20 | INSTALLPATH=$(realpath $1) 21 | 22 | mkdir -p build 23 | cd build 24 | cmake -DCMAKE_INSTALL_PREFIX=${INSTALLPATH} .. 25 | make install 26 | 27 | if [[ $1 = .* ]]; then 28 | echo 29 | echo -e "\033[1;33m'source ${INSTALLPATH}/env.sh' in this folder before building the main library \033[1;0m" 30 | echo 31 | fi 32 | -------------------------------------------------------------------------------- /src/PlannerConfig.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace ugv_nav4d{ 4 | /** 5 | * Describes the planner config of the path planner. 6 | */ 7 | struct PlannerConfig 8 | { 9 | /** Should a computationally expensive obstacle check be done to check whether the robot bounding box 10 | * is in collision with obstacles. This mode is useful for highly cluttered and tight spaced environments */ 11 | bool usePathStatistics = false; 12 | /** Search only until the first solution and then stop planning 13 | * See SBPL documentation for an explantion of this value*/ 14 | bool searchUntilFirstSolution = false; 15 | /** The initial epsilon for the internal ARA* algorithm. 16 | * See SBPL documentation for an explantion of this value*/ 17 | double initialEpsilon = 20.0; 18 | /** The epsilon step size for the internal ARA* algoritm. 19 | * See SBPL documentation for an explantion of this value*/ 20 | double epsilonSteps = 2.0; 21 | /** Number of threads to use during planning */ 22 | unsigned numThreads = 1; 23 | }; 24 | } 25 | -------------------------------------------------------------------------------- /src/gui/Main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include //gcc only 3 | #include "PlannerGui.h" 4 | #include 5 | 6 | 7 | class Application : public QApplication { 8 | public: 9 | Application(int& argc, char** argv) : QApplication(argc, argv) {} 10 | 11 | virtual bool notify(QObject *receiver, QEvent *e) 12 | { 13 | try 14 | { 15 | return QApplication::notify(receiver, e); 16 | } catch (std::exception &ex) 17 | { 18 | LOG_ERROR_S << "CAUGHT exception in qt event loop:\n" << ex.what(); 19 | const int traceLen = 40; 20 | void *symbols[traceLen]; 21 | const size_t size = backtrace(symbols, traceLen); 22 | backtrace_symbols_fd(symbols, size, STDERR_FILENO); 23 | 24 | } catch (...) { 25 | LOG_ERROR_S << "CAUGHT unknown exception in qt event loop"; 26 | } 27 | return false; 28 | } 29 | }; 30 | 31 | int main(int argc, char** argv) 32 | { 33 | Application app(argc, argv); 34 | PlannerGui gui(argc, argv); 35 | gui.show(); 36 | app.exec(); 37 | return 0; 38 | } 39 | 40 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: "1.2.0" 2 | authors: 3 | - family-names: Böckmann 4 | given-names: Arne 5 | - family-names: Machowinski 6 | given-names: Janosch 7 | - family-names: Lodhi 8 | given-names: Muhammad Haider Khan 9 | orcid: "https://orcid.org/0009-0008-1199-3489" 10 | doi: 10.5281/zenodo.14236941 11 | message: If you use this software, please cite our article in the 12 | Journal of Open Source Software. 13 | preferred-citation: 14 | authors: 15 | - family-names: Böckmann 16 | given-names: Arne 17 | - family-names: Machowinski 18 | given-names: Janosch 19 | - family-names: Lodhi 20 | given-names: Muhammad Haider Khan 21 | orcid: "https://orcid.org/0009-0008-1199-3489" 22 | date-published: 2024-12-02 23 | doi: 10.21105/joss.06983 24 | issn: 2475-9066 25 | issue: 104 26 | journal: Journal of Open Source Software 27 | publisher: 28 | name: Open Journals 29 | start: 6983 30 | title: "ugv_nav4d: Advanced Multi-Surface Navigation for Unmanned 31 | Ground Vehicles Using 4D Path Planning Techniques" 32 | type: article 33 | url: "https://joss.theoj.org/papers/10.21105/joss.06983" 34 | volume: 9 35 | title: "ugv_nav4d: Advanced Multi-Surface Navigation for Unmanned Ground 36 | Vehicles Using 4D Path Planning Techniques" 37 | -------------------------------------------------------------------------------- /src/gui/ReplayDump.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include //gcc only 3 | #include "PlannerGui.h" 4 | #include 5 | 6 | class Application : public QApplication { 7 | public: 8 | Application(int& argc, char** argv) : QApplication(argc, argv) {} 9 | 10 | virtual bool notify(QObject *receiver, QEvent *e) 11 | { 12 | try 13 | { 14 | return QApplication::notify(receiver, e); 15 | } catch (std::exception &ex) 16 | { 17 | LOG_ERROR_S << "CAUGHT exception in qt event loop:\n" << ex.what(); 18 | const int traceLen = 40; 19 | void *symbols[traceLen]; 20 | const size_t size = backtrace(symbols, traceLen); 21 | backtrace_symbols_fd(symbols, size, STDERR_FILENO); 22 | 23 | } catch (...) { 24 | LOG_ERROR_S << "CAUGHT unknown exception in qt event loop"; 25 | } 26 | return false; 27 | } 28 | }; 29 | 30 | int main(int argc, char** argv) 31 | { 32 | if(argc < 2) 33 | { 34 | LOG_INFO_S << argv[0] << " [dumpName]"; 35 | return -1; 36 | } 37 | 38 | Application app(argc, argv); 39 | PlannerGui gui(argv[1]); 40 | gui.show(); 41 | app.exec(); 42 | return 0; 43 | } 44 | 45 | -------------------------------------------------------------------------------- /src/DebugDrawingDeclarations.cpp: -------------------------------------------------------------------------------- 1 | #ifdef ENABLE_DEBUG_DRAWINGS 2 | 3 | #include 4 | 5 | // planner debug channels 6 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_obstacle_error"); 7 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_env_startPos"); 8 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_env_goalPos"); 9 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_expandStarts"); 10 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_successors"); 11 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_allowedAngles"); 12 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_expandFailStepHeight"); 13 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_check_start_goal_start"); 14 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_check_start_goal_goal"); 15 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_obst_map"); 16 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_trajectory"); 17 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_greedyPath"); 18 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_check_fail_goal"); 19 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_obs_check_fail_goal"); 20 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_check_fail_start"); 21 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_obs_check_fail_start"); 22 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_goalBox"); 23 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_startBox"); 24 | V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_primitives"); 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /src/test/test_DiscreteTheta.cpp: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_MODULE DiscreteThetaModule 2 | #include 3 | 4 | #include "ugv_nav4d/DiscreteTheta.hpp" 5 | 6 | BOOST_AUTO_TEST_CASE(check_discrete_theta_init) { 7 | DiscreteTheta theta = DiscreteTheta(0, 16); 8 | BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 0, 0.01); 9 | 10 | theta = DiscreteTheta(1, 16); 11 | BOOST_CHECK_EQUAL(theta.getNumAngles(), 16); 12 | BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 0.3926, 0.01); 13 | 14 | theta = DiscreteTheta(-1, 16); 15 | BOOST_CHECK_EQUAL(theta.getTheta(), 15); 16 | BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 5.8904, 0.01); 17 | 18 | theta = DiscreteTheta(18, 16); 19 | BOOST_CHECK_EQUAL(theta.getTheta(), 2); 20 | 21 | theta = DiscreteTheta(16, 16); 22 | BOOST_CHECK_EQUAL(theta.getTheta(), 0); 23 | 24 | theta = DiscreteTheta(5.90, 16); 25 | BOOST_CHECK_EQUAL(theta.getTheta(), 15); 26 | 27 | theta = DiscreteTheta(5.45, 16); 28 | BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 5.45, 0.01); 29 | BOOST_CHECK_EQUAL(theta.getTheta(), 14); 30 | 31 | DiscreteTheta thetaA = DiscreteTheta(3, 16); 32 | DiscreteTheta thetaB = DiscreteTheta(5, 16); 33 | BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 2); 34 | 35 | thetaA = DiscreteTheta(2, 16); 36 | thetaB = DiscreteTheta(14, 16); 37 | BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 4); 38 | } -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(OpenMP REQUIRED) 2 | 3 | set(DEPS_PKGCONFIG_LIST 4 | base-types 5 | sbpl 6 | sbpl_spline_primitives 7 | traversability_generator3d 8 | trajectory_follower 9 | ) 10 | 11 | if(ENABLE_DEBUG_DRAWINGS AND vizkit3d_debug_drawings-commands_FOUND) 12 | list(APPEND DEPS_PKGCONFIG_LIST vizkit3d_debug_drawings-commands) 13 | endif() 14 | 15 | set(ADDITIONAL_LIBS "") 16 | if(OpenMP_CXX_FOUND) 17 | message(STATUS "OpenMP is found: ${OpenMP_CXX_FOUND}") 18 | list(APPEND ADDITIONAL_LIBS OpenMP::OpenMP_CXX) 19 | endif() 20 | 21 | rock_library(ugv_nav4d 22 | SOURCES 23 | DiscreteTheta.cpp 24 | EnvironmentXYZTheta.cpp 25 | PathStatistic.cpp 26 | Planner.cpp 27 | PlannerDump.cpp 28 | PreComputedMotions.cpp 29 | Dijkstra.cpp 30 | DebugDrawingDeclarations.cpp 31 | HEADERS 32 | Mobility.hpp 33 | DiscreteTheta.hpp 34 | EnvironmentXYZTheta.hpp 35 | PathStatistic.hpp 36 | Planner.hpp 37 | PlannerConfig.hpp 38 | PreComputedMotions.hpp 39 | Dijkstra.hpp 40 | DEPS_PKGCONFIG 41 | ${DEPS_PKGCONFIG_LIST} 42 | LIBS 43 | ${ADDITIONAL_LIBS} 44 | ) 45 | 46 | add_subdirectory(gui) 47 | 48 | if(TESTS_ENABLED) 49 | message(STATUS "TESTS_ENABLED is defined with value: ${TESTS_ENABLED}") 50 | add_subdirectory(test) 51 | else() 52 | message(STATUS "TESTS_ENABLED is set to OFF. Skipped!") 53 | endif() 54 | -------------------------------------------------------------------------------- /.github/workflows/docs.yml: -------------------------------------------------------------------------------- 1 | name: C/C++ CI 2 | 3 | on: 4 | push: 5 | branches: [ "main", "feat-build-doc" ] 6 | pull_request: 7 | branches: [ "main" ] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | steps: 13 | - uses: actions/checkout@v4 14 | - name: Install dependencies 15 | run: sudo apt-get update && sudo apt-get install -y graphviz doxygen 16 | shell: bash 17 | 18 | - name: build_doxygen 19 | run: | 20 | # bootstrap rock 21 | git clone https://github.com/rock-core/base-cmake.git 22 | cmake -Bbuild/rock base-cmake -DCMAKE_INSTALL_PREFIX=build/install 23 | make -Cbuild/rock install 24 | export CMAKE_PREFIX_PATH=build/install/share/rock/cmake/:$CMAKE_PREFIX_PATH 25 | # get doxygen to trigger generation 26 | # build the doxyfile 27 | cmake -Bbuild . || true # building doc does not need build deps 28 | 29 | - name: move doc with figures into a tmp folder 30 | run: mv doc doc_tmp 31 | 32 | - name: Generate Doxygen Documentation 33 | run: doxygen build/Doxyfile 34 | shell: bash 35 | 36 | - name: Create .nojekyll (ensures pages with underscores work on gh pages) 37 | run: touch doc/.nojekyll 38 | shell: bash 39 | 40 | - name: move figures into the actual doc 41 | run: mv doc_tmp doc/doc 42 | 43 | - name: Deploy to GitHub Pages 44 | uses: JamesIves/github-pages-deploy-action@v4 45 | with: 46 | token: ${{ secrets.GITHUB_TOKEN }} 47 | branch: gh-pages 48 | folder: doc 49 | 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, DFKI Planning 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists.txt has to be located in the project folder and cmake has to be 2 | # executed from 'project/build' with 'cmake ../'. 3 | 4 | cmake_minimum_required(VERSION 3.9) 5 | project(ugv_nav4d 6 | VERSION 1.0 7 | DESCRIPTION "A 4D (X,Y,Z, Theta) Planner for unmaned ground vehicles (UGVs).") 8 | set(CMAKE_CXX_STANDARD 14) 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 10 | 11 | if(INSTALL_DEPS) 12 | 13 | execute_process(COMMAND bash install_os_dependencies.bash 14 | WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/source_dependencies) 15 | execute_process(COMMAND bash build.bash ${CMAKE_INSTALL_PREFIX} 16 | WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/source_dependencies) 17 | set(ENV{PKG_CONFIG_PATH} ${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/) 18 | set(ENV{CMAKE_PREFIX_PATH} ${CMAKE_INSTALL_PREFIX}/share/rock/cmake/) 19 | endif() 20 | 21 | 22 | find_package(Rock) 23 | rock_init() 24 | rock_feature(NOCURDIR) 25 | 26 | pkg_check_modules(vizkit3d_debug_drawings-commands vizkit3d_debug_drawings-commands) 27 | 28 | if(ENABLE_DEBUG_DRAWINGS) 29 | message(STATUS "ENABLE_DEBUG_DRAWINGS is defined with value: ${ENABLE_DEBUG_DRAWINGS}") 30 | if(vizkit3d_debug_drawings-commands_FOUND) 31 | add_definitions(-DENABLE_DEBUG_DRAWINGS) 32 | message(STATUS "Building with vizkit3d_debug_drawings support in library") 33 | else() 34 | message(STATUS "Cannot find vizkit3d_debug_drawings, ignoring ENABLE_DEBUG_DRAWINGS") 35 | endif() 36 | else() 37 | message(STATUS "ENABLE_DEBUG_DRAWINGS is set to OFF. Skipped!") 38 | endif() 39 | 40 | rock_standard_layout() 41 | -------------------------------------------------------------------------------- /src/PlannerDump.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "Mobility.hpp" 7 | #include 8 | #include "Planner.hpp" 9 | 10 | #include 11 | 12 | namespace ugv_nav4d { 13 | 14 | class Planner; 15 | 16 | class PlannerDump 17 | { 18 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 19 | Mobility mobility; 20 | traversability_generator3d::TraversabilityConfig traversabilityConfig; 21 | PlannerConfig plannerConfig; 22 | 23 | base::samples::RigidBodyState start; 24 | base::samples::RigidBodyState goal; 25 | base::Time maxTime; 26 | 27 | std::string getUnusedFilename(const std::string& filePostfix) const; 28 | 29 | traversability_generator3d::TravMap3d travMap; 30 | 31 | public: 32 | 33 | /** 34 | * Constructor for loading 35 | * */ 36 | PlannerDump(const std::string &dumpName); 37 | 38 | PlannerDump(const ugv_nav4d::Planner& planner, const std::string& filePostfix, const base::Time& maxTime, const base::samples::RigidBodyState& startbody2Mls, const base::samples::RigidBodyState& endbody2Mls); 39 | 40 | const sbpl_spline_primitives::SplinePrimitivesConfig &getSplineConfig() const 41 | { 42 | return splinePrimitiveConfig; 43 | } 44 | 45 | const Mobility &getMobilityConf() const 46 | { 47 | return mobility; 48 | } 49 | 50 | const traversability_generator3d::TraversabilityConfig &getTravConfig() const 51 | { 52 | return traversabilityConfig; 53 | } 54 | 55 | const PlannerConfig &getPlannerConfig() const 56 | { 57 | return plannerConfig; 58 | } 59 | 60 | const base::samples::RigidBodyState &getStart() const 61 | { 62 | return start; 63 | } 64 | const base::samples::RigidBodyState &getGoal() const 65 | { 66 | return goal; 67 | } 68 | const base::Time &getMaxTime() const 69 | { 70 | return maxTime; 71 | } 72 | const traversability_generator3d::TravMap3d& getTravMap() const 73 | { 74 | return travMap; 75 | } 76 | 77 | }; 78 | 79 | } 80 | -------------------------------------------------------------------------------- /src/Dijkstra.cpp: -------------------------------------------------------------------------------- 1 | #include "Dijkstra.hpp" 2 | #include 3 | #include 4 | #include 5 | using namespace maps::grid; 6 | 7 | namespace ugv_nav4d 8 | { 9 | void Dijkstra::computeCost(const TraversabilityNodeBase* source, 10 | std::unordered_map& outDistances, 11 | const traversability_generator3d::TraversabilityConfig& config) 12 | { 13 | outDistances.clear(); 14 | outDistances[source] = 0.0; 15 | 16 | // Use a priority queue for efficiency 17 | std::priority_queue, 18 | std::vector>, 19 | std::greater<>> vertexQ; 20 | vertexQ.emplace(0.0, source); 21 | 22 | std::unordered_set visited; 23 | 24 | while (!vertexQ.empty()) 25 | { 26 | const double dist = vertexQ.top().first; 27 | const TraversabilityNodeBase* u = vertexQ.top().second; 28 | vertexQ.pop(); 29 | 30 | // Skip nodes already processed 31 | if (visited.find(u) != visited.end()) 32 | continue; 33 | visited.insert(u); 34 | 35 | const Eigen::Vector3d uPos(u->getIndex().x() * config.gridResolution, 36 | u->getIndex().y() * config.gridResolution, 37 | u->getHeight()); 38 | 39 | // Visit each edge exiting u 40 | for (TraversabilityNodeBase* v : u->getConnections()) 41 | { 42 | if (v->getType() != TraversabilityNodeBase::TRAVERSABLE) 43 | continue; 44 | 45 | const Eigen::Vector3d vPos(v->getIndex().x() * config.gridResolution, 46 | v->getIndex().y() * config.gridResolution, 47 | v->getHeight()); 48 | 49 | const double distance = (vPos - uPos).norm(); 50 | double distance_through_u = dist + distance; 51 | 52 | if (outDistances.find(v) == outDistances.end() || distance_through_u < outDistances[v]) 53 | { 54 | outDistances[v] = distance_through_u; 55 | vertexQ.emplace(outDistances[v], v); 56 | } 57 | } 58 | } 59 | } 60 | } 61 | 62 | -------------------------------------------------------------------------------- /paper/paper.bib: -------------------------------------------------------------------------------- 1 | @software{slammaps, 2 | title = {{SLAM Maps}}, 3 | author = {Anna Born and Sascha Arnold}, 4 | year = {2015}, 5 | url = {https://github.com/envire/slam-maps}, 6 | } 7 | 8 | @software{sbpl, 9 | title = {{SBPL: Search-Based Planning Library}}, 10 | author = {Maxim Likhachev and Dave Ferguson and Geoff Gordon}, 11 | year = {2005}, 12 | version = {1.3}, 13 | url = {https://github.com/sbpl/sbpl}, 14 | } 15 | 16 | @software{travgen3d, 17 | title = {{Traversability Generator3d}}, 18 | author = {Janosch Machowinski and Arne Böckmann}, 19 | url = {https://github.com/dfki-ric/traversability_generator3d}, 20 | note = {Accessed: June 20, 2024}, 21 | year = {2016}, 22 | } 23 | 24 | @misc{roboticsElab, 25 | title = {{Robotics Exploration Laboratory}}, 26 | url = {https://www.dfki.de/web/technologien-anwendungen/living-labs/robotics-exploration-laboratory}, 27 | note = {Accessed: June 20, 2024}, 28 | year = {2024} 29 | } 30 | 31 | @software{trajectoryfollower, 32 | title = {{Trajectory Follower}}, 33 | author = {Ajish Babu}, 34 | url = {https://github.com/rock-control/control-trajectory_follower}, 35 | note = {Accessed: June 20, 2024}, 36 | year = {2016}, 37 | } 38 | 39 | @software{sbplsplineprimitives, 40 | title = {{SBPL Spline Primitives}}, 41 | author = {Arne Böckmann}, 42 | url = {https://github.com/rock-planning/planning-sbpl_spline_primitives}, 43 | note = {Accessed: June 20, 2024}, 44 | year = {2016}, 45 | } 46 | 47 | @INPROCEEDINGS{mlsmaps, 48 | title={Multi-Level Surface Maps for Outdoor Terrain Mapping and Loop Closing}, 49 | author={Triebel, Rudolph and Pfaff, Patrick and Burgard, Wolfram}, 50 | booktitle={2006 IEEE/RSJ International Conference on Intelligent Robots and Systems}, 51 | year={2006}, 52 | pages={2276-2282}, 53 | keywords={Terrain mapping;Mobile robots;Data structures;Path planning;Multilevel systems;Bridges;Buildings;Impedance matching;Robot sensing systems;Large-scale systems}, 54 | doi={10.1109/IROS.2006.282632} 55 | } 56 | 57 | @article{macenski2020nav2, 58 | title = {{The Marathon 2: {A} Navigation System}}, 59 | author = {Steve Macenski and 60 | Francisco Mart{\'{\i}}n and 61 | Ruffin White and 62 | Jonatan Gin{\'{e}}s Clavero}, 63 | journal = {CoRR}, 64 | volume = {abs/2003.00368}, 65 | year = {2020}, 66 | url = {https://arxiv.org/abs/2003.00368}, 67 | doi = {10.1109/IROS45743.2020.9341207}, 68 | eprinttype = {arXiv}, 69 | eprint = {2003.00368}, 70 | timestamp = {Wed, 11 Mar 2020 09:34:48 +0100}, 71 | biburl = {https://dblp.org/rec/journals/corr/abs-2003-00368.bib}, 72 | bibsource = {dblp computer science bibliography, https://dblp.org} 73 | } -------------------------------------------------------------------------------- /src/DiscreteTheta.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | class DiscreteTheta 7 | { 8 | int theta; 9 | int numAngles; 10 | 11 | void normalize() 12 | { 13 | assert(numAngles >= 0); 14 | if(theta < 0) 15 | { 16 | int num = (theta / -numAngles) + 1; 17 | theta += numAngles * num; 18 | } 19 | 20 | if(theta >= numAngles) 21 | theta = theta % numAngles; 22 | } 23 | 24 | public: 25 | DiscreteTheta(int val, unsigned int numAngles) : theta(val) , numAngles(numAngles) { 26 | normalize(); 27 | } 28 | 29 | DiscreteTheta(double val, unsigned int numAngles) : numAngles(numAngles) { 30 | theta = round((val * numAngles) / (2.0 * M_PI)); 31 | normalize(); 32 | } 33 | 34 | DiscreteTheta(const DiscreteTheta &o) : theta(o.theta), numAngles(o.numAngles) { 35 | } 36 | 37 | DiscreteTheta& operator+=(const DiscreteTheta& rhs) 38 | { 39 | theta += rhs.theta; 40 | normalize(); 41 | return *this; 42 | } 43 | 44 | DiscreteTheta& operator-=(const DiscreteTheta& rhs) 45 | { 46 | theta -= rhs.theta; 47 | normalize(); 48 | return *this; 49 | } 50 | 51 | friend DiscreteTheta operator+(DiscreteTheta lhs, const DiscreteTheta& rhs) 52 | { 53 | lhs += rhs; 54 | return lhs; 55 | } 56 | 57 | friend DiscreteTheta operator-(DiscreteTheta lhs, const DiscreteTheta& rhs) 58 | { 59 | lhs -= rhs; 60 | return lhs; 61 | } 62 | 63 | friend bool operator<(const DiscreteTheta& l, const DiscreteTheta& r) 64 | { 65 | return l.theta < r.theta; 66 | } 67 | 68 | friend bool operator==(const DiscreteTheta& l, const DiscreteTheta& r) 69 | { 70 | return l.theta == r.theta; 71 | } 72 | 73 | int getTheta() const 74 | { 75 | return theta; 76 | } 77 | 78 | double getRadian() const 79 | { 80 | return M_PI * 2.0 * theta / static_cast(numAngles); 81 | } 82 | 83 | int getNumAngles() const 84 | { 85 | return numAngles; 86 | } 87 | 88 | DiscreteTheta shortestDist(const DiscreteTheta &ain) const 89 | { 90 | DiscreteTheta diffA = ain-*this; 91 | 92 | int a = diffA.theta; 93 | int b = numAngles - diffA.theta; 94 | 95 | 96 | if(a < b) 97 | return DiscreteTheta(a, numAngles); 98 | 99 | return DiscreteTheta(b, numAngles); 100 | } 101 | }; 102 | 103 | std::ostream& operator<< (std::ostream& stream, const DiscreteTheta& angle); 104 | -------------------------------------------------------------------------------- /src/gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | rock_find_qt4(OPTIONAL) 2 | rock_find_qt5(OPTIONAL) 3 | 4 | include(RockQt) 5 | 6 | find_package(PCL 1.7 REQUIRED COMPONENTS common io) 7 | IF("${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}" VERSION_LESS 1.14) 8 | SET(PCL_VERSION_SUFFIX "-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}") 9 | ENDIF() 10 | 11 | if(ENABLE_DEBUG_DRAWINGS AND vizkit3d_debug_drawings-commands_FOUND) 12 | # These are (only) required for building the GUIs: 13 | list(APPEND DEPS_PKGCONFIG_QT vizkit3d_debug_drawings) 14 | endif() 15 | 16 | rock_qt_library( 17 | TARGETPREFIX ugv_nav4d_gui 18 | QT4_SUFFIX "" 19 | MISSINGQTDEPS_NOBUILD 20 | SOURCES 21 | PlannerGui.cpp 22 | HEADERS 23 | PlannerGui.h 24 | MOC 25 | PlannerGui.h 26 | DEPS ugv_nav4d 27 | DEPS_QT4 Qt4::QtCore 28 | DEPS_QT5 Qt5::Core 29 | DEPS_PKGCONFIG 30 | base-types 31 | pcl_common${PCL_VERSION_SUFFIX} 32 | pcl_io${PCL_VERSION_SUFFIX} 33 | DEPS_PKGCONFIG_QT vizkit3d 34 | vizkit3d-viz 35 | traversability_generator3d-viz 36 | maps-viz 37 | base-viz 38 | sbpl_spline_primitives-viz 39 | trajectory_follower-viz 40 | ${DEPS_PKGCONFIG_QT} 41 | ) 42 | 43 | rock_qt_executable( 44 | TARGETPREFIX ugv_nav4d_bin 45 | QT4_SUFFIX "" 46 | MISSINGQTDEPS_NOBUILD 47 | SOURCES 48 | Main.cpp 49 | DEPS ugv_nav4d 50 | DEPS_QT ugv_nav4d_gui 51 | ) 52 | 53 | rock_qt_executable( 54 | TARGETPREFIX ugv_nav4d_replay 55 | QT4_SUFFIX "" 56 | MISSINGQTDEPS_NOBUILD 57 | SOURCES 58 | ReplayDump.cpp 59 | DEPS ugv_nav4d 60 | DEPS_QT ugv_nav4d_gui 61 | DEPS_PKGCONFIG 62 | base-types 63 | DEPS_PKGCONFIG_QT vizkit3d maps-viz base-viz 64 | sbpl_spline_primitives-viz trajectory_follower-viz 65 | ) 66 | 67 | if(TARGET ugv_nav4d_gui) 68 | if (ENABLE_DEBUG_DRAWINGS AND vizkit3d_debug_drawings-commands_FOUND AND vizkit3d_debug_drawings_FOUND) 69 | message(STATUS "Gui using Qt4: enabled, with debug drawings") 70 | else() 71 | message(STATUS "Gui using Qt4: enabled, without debug drawings") 72 | endif() 73 | else(ROCK_QT_VERSION_4 AND vizkit3d_FOUND) 74 | message(STATUS "Gui using Qt4: disabled") 75 | endif() 76 | 77 | if(TARGET ugv_nav4d_gui-qt5) 78 | if (ENABLE_DEBUG_DRAWINGS AND vizkit3d_debug_drawings-commands_FOUND AND vizkit3d_debug_drawings-qt5_FOUND) 79 | message(STATUS "Gui using Qt5: enabled, with debug drawings") 80 | else() 81 | message(STATUS "Gui using Qt5: enabled, without debug drawings") 82 | endif() 83 | else() 84 | message(STATUS "Gui using Qt5: disabled") 85 | endif() 86 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing to ugv_nav4d 2 | 3 | Please inform the maintainer as early as possible about your planned 4 | feature developments, extensions, or bugfixes that you are working on. 5 | An easy way is to open an issue or a pull request in which you explain 6 | what you are trying to do. 7 | 8 | ## Pull Requests 9 | 10 | The preferred way to contribute to ugv_nav4d is to fork the 11 | [main repository](https://github.com/dfki-ric/ugv_nav4d) on GitHub, then submit a "pull request" 12 | (PR): 13 | 14 | 1. [Create an account](https://github.com/signup/free) on 15 | GitHub if you do not already have one. 16 | 17 | 2. Fork the [project repository](https://github.com/dfki-ric/ugv_nav4d): 18 | click on the 'Fork' button near the top of the page. This creates a copy of 19 | the code under your account on the GitHub server. 20 | 21 | 3. Clone this copy to your local disk: 22 | 23 | $ git clone git@github.com:YourLogin/ugv_nav4d.git 24 | 25 | 4. Create a branch to hold your changes: 26 | 27 | $ git checkout -b my-feature 28 | 29 | and start making changes. Never work in the ``main`` branch! 30 | 31 | 5. Work on this copy, on your computer, using Git to do the version 32 | control. When you're done editing, do:: 33 | 34 | $ git add modified_files 35 | $ git commit 36 | 37 | to record your changes in Git, then push them to GitHub with:: 38 | 39 | $ git push -u origin my-feature 40 | 41 | Finally, go to the web page of the your fork of the ugv_nav4d repo, 42 | and click 'Pull request' to send your changes to the maintainers for review. 43 | request. 44 | 45 | ## Merge Policy 46 | 47 | Usually it is not possible to push directly to the master branch of Software Name 48 | for anyone. Only tiny changes, urgent bugfixes, and maintenance commits can 49 | be pushed directly to the master branch by the maintainer without a review. 50 | "Tiny" means backwards compatibility is mandatory and all tests must succeed. 51 | No new feature must be added. 52 | 53 | Developers have to submit pull requests. Those will be reviewed by at least 54 | one other developer and merged by the maintainer. New features must be 55 | documented and tested. Breaking changes must be discussed and announced 56 | in advance with deprecation warnings. 57 | 58 | ## Project Roadmap 59 | 60 | Please see the issues for planned future changes. 61 | 62 | ## Funding 63 | The ugv_nav4d library was initiated and is currently developed at the Robotics Innovation Center of the German Research Center for Artificial Intelligence (DFKI) in Bremen, together with the Robotics Group of the University of Bremen. The development was started in the scope of the Entern project (50RA1406), which has been funded by the German Aerospace Center (DLR) with funds from the German Federal Ministry for Economic Affairs and Climate Action (BMWK). 64 | 65 | -------------------------------------------------------------------------------- /src/PathStatistic.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace ugv_nav4d 8 | { 9 | 10 | /** Calculates statistics about different patches encountered while 11 | * traversing a path. 12 | * I.e. counts patchtes that the robot will traverase when following a path */ 13 | class PathStatistic 14 | { 15 | const traversability_generator3d::TraversabilityConfig &config; 16 | public: 17 | 18 | class Stats 19 | { 20 | size_t obstacles; 21 | double minDistToObstacle; 22 | size_t frontiers; 23 | 24 | /** Minimum distance to each patch type. 25 | * Indexed by patch type. */ 26 | std::vector minDistance; 27 | public: 28 | Stats(); 29 | 30 | size_t getNumObstacles() const 31 | { 32 | return obstacles; 33 | }; 34 | 35 | double getMinDistToObstacles() const; 36 | 37 | size_t getNumFrontiers() const 38 | { 39 | return frontiers; 40 | }; 41 | double getMinDistToFrontiers() const; 42 | void updateStatistic(const maps::grid::TraversabilityNodeBase* node); 43 | void updateDistance(const maps::grid::TraversabilityNodeBase* node, double distance); 44 | }; 45 | 46 | protected: 47 | /** Statistics about patches that the robot traverses while following a given path */ 48 | Stats robotStats; 49 | /** Statistics about patches in the corridor outside the robot bounding box */ 50 | Stats boundaryStats; 51 | public: 52 | 53 | PathStatistic(const traversability_generator3d::TraversabilityConfig &config); 54 | 55 | /** 56 | * Accumulates statistics about all patches that are inside a config.costFunctionDist wide corridor around @p path. 57 | * This can be used to answer questions such as: 58 | * - How many obstacles does the robot traverse when following this path 59 | * - How many obstacles are close to the robot (but are not directly traversed) when following this path (boundaryStats.getNumObstacles()) 60 | * - How many frontiers does the robot traverse when following this path (robotStats.getNumFrontiers()) 61 | * - Is it safe to follow this path? (robotStats.getNumObstacles() == 0?) 62 | * - What is the minimum distance to an obstacle on this path? 63 | * - etc. 64 | * 65 | * The statistics are used extensively during planning. 66 | * 67 | * @param path List of patches that define the path. 68 | * @param poses List of poses on the path. There should be one pose for each patch. 69 | * @param debugObstacleName optional name of the debug drawing that should be used for this path 70 | */ 71 | void calculateStatistics(const std::vector &path, 72 | const std::vector &poses, 73 | const traversability_generator3d::TravMap3d& trMap, 74 | const std::string &debugObstacleName = std::string()); 75 | 76 | bool isPathFeasible(const std::vector &path, 77 | const std::vector &poses, 78 | const traversability_generator3d::TravMap3d& trMap); 79 | 80 | const Stats &getRobotStats() const 81 | { 82 | return robotStats; 83 | } 84 | 85 | const Stats &getBoundaryStats() const 86 | { 87 | return boundaryStats; 88 | } 89 | 90 | }; 91 | 92 | } 93 | -------------------------------------------------------------------------------- /src/Mobility.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace ugv_nav4d{ 4 | 5 | //ATTENTION Mobility is copied from motion_planning_libraries to avoid the dependency. Coping a simple struct is better than getting all the dependencies... 6 | 7 | /** 8 | * Describes the mobility of the system. 9 | * The minimal turning radius is used to create valid curves. 10 | * The multipliers are used by SBPL during planning. 11 | */ 12 | struct Mobility { 13 | // Defines the forward/backward speed of the robot. Is used for cost calculations 14 | double translationSpeed; // m/sec. 15 | // Defines the rotational speed of the robot. Is used for cost calculations. 16 | double rotationSpeed; // rad/sec 17 | // If > 0 allows to specify the minimal turning radius of the system in meter. 18 | // Without this not valid curves may be created. 19 | double minTurningRadius; 20 | // Resolution used to sample the motion primitive spline 21 | double spline_sampling_resolution; 22 | // Remove the goal offset which is there because of the discretization 23 | bool remove_goal_offset; 24 | /** The cost of a motion is multiplied by one of the following Multipliers. This allows 25 | * the user to penalize some motion types. 26 | * Do ***not*** set the multiplier to zero. If you do all motions of that type will have no cost */ 27 | unsigned int multiplierForward; 28 | unsigned int multiplierBackward; 29 | unsigned int multiplierLateral; 30 | unsigned int multiplierForwardTurn; 31 | unsigned int multiplierBackwardTurn; 32 | unsigned int multiplierPointTurn; 33 | unsigned int multiplierLateralCurve; 34 | 35 | // If > 0 a new goal would be searched, if the choosen goal is invalid 36 | double searchRadius; 37 | // The search circle is increased by this value after each unsuccessful case. This option determines the step size between two search radii. 38 | double searchProgressSteps; 39 | 40 | double maxMotionCurveLength; 41 | 42 | 43 | Mobility() : 44 | translationSpeed(1.0), 45 | rotationSpeed(1.0), 46 | minTurningRadius(1.0), 47 | spline_sampling_resolution(0.05), 48 | remove_goal_offset(false), 49 | multiplierForward(1), 50 | multiplierBackward(2), 51 | multiplierLateral(4), 52 | multiplierForwardTurn(2), 53 | multiplierBackwardTurn(3), 54 | multiplierPointTurn(3), 55 | multiplierLateralCurve(4), 56 | searchRadius(1.0), 57 | searchProgressSteps(0.1), 58 | maxMotionCurveLength(100) 59 | { 60 | } 61 | 62 | Mobility(double speed, 63 | double turning_speed, 64 | double min_turning_radius, 65 | double sampling_resolution, 66 | bool correct_goal_offset, 67 | unsigned int mult_forward=0, 68 | unsigned int mult_backward=0, 69 | unsigned int mult_lateral=0, 70 | unsigned int mult_forward_turn=0, 71 | unsigned int mult_backward_turn=0, 72 | unsigned int mult_pointturn=0, 73 | unsigned int mult_lateral_curve=0, 74 | double search_radius = 0, 75 | double search_progress_steps = 0, 76 | double max_motion_curve_length = 0 77 | ) : 78 | translationSpeed(speed), 79 | rotationSpeed(turning_speed), 80 | minTurningRadius(min_turning_radius), 81 | spline_sampling_resolution(sampling_resolution), 82 | remove_goal_offset(correct_goal_offset), 83 | multiplierForward(mult_forward), 84 | multiplierBackward(mult_backward), 85 | multiplierLateral(mult_lateral), 86 | multiplierForwardTurn(mult_forward_turn), 87 | multiplierBackwardTurn(mult_backward_turn), 88 | multiplierPointTurn(mult_pointturn), 89 | multiplierLateralCurve(mult_lateral_curve), 90 | searchRadius(search_radius), 91 | searchProgressSteps(search_progress_steps), 92 | maxMotionCurveLength(max_motion_curve_length) 93 | { 94 | } 95 | }; 96 | 97 | 98 | } 99 | -------------------------------------------------------------------------------- /src/gui/PlannerGui.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #ifndef Q_MOC_RUN 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #endif 20 | 21 | class QDoubleSpinBox; 22 | class QSpinBox; 23 | class QSlider; 24 | class QPushButton; 25 | class QComboBox; 26 | class QProgressBar; 27 | 28 | namespace vizkit3d { 29 | class Vizkit3DWidget; 30 | } 31 | 32 | class PlannerGui : public QObject 33 | { 34 | Q_OBJECT; 35 | 36 | void setupPlanner(int argc, char** argv); 37 | void setupUI(); 38 | 39 | public: 40 | PlannerGui(int argc, char** argv); 41 | PlannerGui(const std::string &dumpName); 42 | 43 | void show(); 44 | public slots: 45 | /** Called when the user clicks a patch on the mls */ 46 | void picked(float x, float y,float z, int buttonMask, int modifierMask); 47 | 48 | //display the planner results 49 | void plannerIsDone(); 50 | 51 | /**plan from @p staro to @p goal */ 52 | void plan(const base::Pose& start, const base::Pose& goal); 53 | 54 | signals: 55 | //is emitted if the planner thread is done 56 | void plannerDone(); 57 | 58 | private slots: 59 | void maxSlopeEditingFinished(); 60 | void inclineLimittingLimitSpinBoxEditingFinished(); 61 | void inclineLimittingMinSlopeSpinBoxEditingFinished(); 62 | void slopeMetricScaleSpinBoxEditingFinished(); 63 | void startOrientationChanged(int newValue); 64 | void goalOrientationChanged(int newValue); 65 | void timeEditingFinished(); 66 | void replanButtonReleased(); 67 | void dumpPressed(); 68 | void slopeMetricComboBoxIndexChanged(int index); 69 | void numThreadsValueChanged(int newValue); 70 | void obstacleDistanceSpinBoxEditingFinished(); 71 | void obstacleFactorSpinBoxEditingFinished(); 72 | 73 | private: 74 | void loadMls(); 75 | void loadMls(const std::string& path); 76 | void startPlanThread(); 77 | 78 | private: 79 | 80 | std::atomic inplanningphase{false}; // Atomic for thread-safe flag 81 | vizkit3d::Vizkit3DWidget* widget; 82 | QDoubleSpinBox* maxSlopeSpinBox; 83 | QDoubleSpinBox* slopeMetricScaleSpinBox; 84 | QDoubleSpinBox* time; 85 | QDoubleSpinBox* inclineLimittingMinSlopeSpinBox; 86 | QDoubleSpinBox* inclineLimittingLimitSpinBox; 87 | QSlider* startOrientatationSlider; 88 | QSlider* goalOrientationSlider; 89 | QDoubleSpinBox* obstacleDistanceSpinBox; 90 | QDoubleSpinBox* obstacleFactorSpinBox; 91 | QComboBox* slopeMetricComboBox; 92 | QComboBox* heuristicComboBox; 93 | QSpinBox* numThreadsSpinBox; 94 | QProgressBar* bar; 95 | QWidget window; 96 | vizkit3d::SbplSplineVisualization splineViz; 97 | vizkit3d::SubTrajectoryVisualization trajViz; 98 | vizkit3d::SubTrajectoryVisualization trajViz2; 99 | vizkit3d::MLSMapVisualization mlsViz; 100 | vizkit3d::TravMap3dVisualization trav3dViz; 101 | vizkit3d::RigidBodyStateVisualization startViz; 102 | vizkit3d::RigidBodyStateVisualization goalViz; 103 | vizkit3d::GridVisualization gridViz; 104 | maps::grid::MLSMapSloped mlsMap; 105 | base::Pose start; 106 | base::Pose goal; 107 | bool pickStart = true; 108 | bool startPicked = false; 109 | bool goalPicked = false; 110 | bool threadRunning = false; 111 | bool usingPlannerDump = false; 112 | sbpl_spline_primitives::SplinePrimitivesConfig splineConfig; 113 | ugv_nav4d::Mobility mobilityConfig; 114 | traversability_generator3d::TraversabilityConfig travConfig; 115 | ugv_nav4d::PlannerConfig plannerConfig; 116 | std::shared_ptr planner; //is pointer cause of lazy init 117 | std::vector path; 118 | std::vector beautifiedPath; 119 | std::shared_ptr travGen; 120 | }; 121 | -------------------------------------------------------------------------------- /src/PreComputedMotions.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "DiscreteTheta.hpp" 4 | #include "Mobility.hpp" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace ugv_nav4d 13 | { 14 | 15 | struct PoseWithCell 16 | { 17 | base::Pose2D pose; 18 | maps::grid::Index cell; 19 | }; 20 | 21 | struct CellWithPoses 22 | { 23 | maps::grid::Index cell; 24 | std::vector poses; 25 | }; 26 | 27 | 28 | class Motion 29 | { 30 | public: 31 | 32 | /**used to scale the costs because costs 33 | * are int but real costs are most likely small doubles*/ 34 | static double costScaleFactor; 35 | 36 | enum Type { 37 | MOV_FORWARD, 38 | MOV_BACKWARD, 39 | MOV_POINTTURN, 40 | MOV_LATERAL, 41 | }; 42 | 43 | Motion(unsigned int numAngles = 0) : endTheta(0, numAngles),startTheta(0, numAngles), baseCost(0), id(std::numeric_limits::max()) {}; 44 | 45 | static int calculateCost(double translationalDist, double angularDist, double translationVelocity, double angularVelocity, double costMultiplier); 46 | 47 | int xDiff; 48 | int yDiff; 49 | DiscreteTheta endTheta; 50 | DiscreteTheta startTheta; 51 | 52 | Type type; 53 | 54 | /**the intermediate poses are not discrete. 55 | * They are relative to the starting cell. 56 | * The Poses start from (0/0), while the 57 | * cell idx is computed from the center of the start cell + pose 58 | */ 59 | std::vector intermediateStepsTravMap; 60 | 61 | /** 62 | * This vector contains a full resolution 63 | * sample of the motion primitive, together 64 | * with the cell the poses are supposed to 65 | * be in. Poses are relative to (0/0), while 66 | * the cellIndex is computed relative to the 67 | * center of the start cell. 68 | * */ 69 | std::vector fullSplineSamples; 70 | 71 | int baseCost; //time the robot needs to follow the primivite scaled by some factors 72 | int costMultiplier;//is used to scale the baseCost (can be used to punish certain motions) 73 | double translationlDist; //translational length of the motion 74 | double angularDist; //angular length of the motion 75 | 76 | size_t id; 77 | 78 | }; 79 | 80 | class PreComputedMotions 81 | { 82 | //indexed by discrete start theta 83 | std::vector > thetaToMotion; 84 | std::vector idToMotion; 85 | sbpl_spline_primitives::SbplSplineMotionPrimitives primitives; 86 | Mobility mobilityConfig; 87 | public: 88 | /**Initialize using spline based primitives. 89 | * @param mobilityConfig Will be used to configure and filter the splines. 90 | * mobilityConfig.mMinTurningRadius will be used to 91 | * filter the splines and remove all splines with a smaller turning radius.*/ 92 | PreComputedMotions(const sbpl_spline_primitives::SplinePrimitivesConfig& primitiveConfig, 93 | const Mobility& mobilityConfig); 94 | 95 | void readMotionPrimitives(const sbpl_spline_primitives::SbplSplineMotionPrimitives& primGen, 96 | const Mobility& mobilityConfig, double travGridResolution); 97 | 98 | void computeMotions(double travGridResolution); 99 | 100 | void setMotionForTheta(const Motion &motion, const DiscreteTheta &theta); 101 | 102 | void preComputeCost(Motion &motion); 103 | 104 | const std::vector &getMotionForStartTheta(const DiscreteTheta &theta) const; 105 | 106 | const Motion &getMotion(std::size_t id) const; 107 | 108 | const sbpl_spline_primitives::SbplSplineMotionPrimitives& getPrimitives() const; 109 | 110 | /**Calculate the curvature of a circle based on the radius of the circle */ 111 | static double calculateCurvatureFromRadius(const double r); 112 | private: 113 | 114 | void sampleOnResolution(double gridResolution, base::geometry::Spline2 spline, std::vector< ugv_nav4d::PoseWithCell >& result, std::vector< ugv_nav4d::CellWithPoses >& fullResult); 115 | 116 | base::Pose2D getPointClosestToCellMiddle(const ugv_nav4d::CellWithPoses& cwp, const double gridResolution); 117 | 118 | 119 | void computeSplinePrimCost(const sbpl_spline_primitives::SplinePrimitive& prim, 120 | const Mobility& mobilityConfig, Motion& outMotion) const; 121 | 122 | }; 123 | 124 | 125 | } 126 | -------------------------------------------------------------------------------- /src/PlannerDump.cpp: -------------------------------------------------------------------------------- 1 | #include "PlannerDump.hpp" 2 | #include "Planner.hpp" 3 | #define WRITE(X) output.write(reinterpret_cast(&X), sizeof X) 4 | #define READ(X) input.read(reinterpret_cast(&X), sizeof X) 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | ugv_nav4d::PlannerDump::PlannerDump(const std::string& dumpName) 12 | { 13 | LOG_INFO_S << "Loading Dump " << dumpName; 14 | 15 | std::ifstream input(dumpName, std::ios::binary | std::ios::in); 16 | 17 | READ(traversabilityConfig); 18 | READ(mobility); 19 | READ(splinePrimitiveConfig); 20 | READ(plannerConfig); 21 | base::Pose tmp; 22 | READ(tmp); 23 | start.setPose(tmp); 24 | READ(tmp); 25 | goal.setPose(tmp); 26 | double maxTimed; 27 | READ(maxTimed); 28 | 29 | boost::archive::binary_iarchive ia(input); 30 | ia >> travMap; 31 | } 32 | 33 | ugv_nav4d::PlannerDump::PlannerDump(const ugv_nav4d::Planner& planner, 34 | const std::string& filePostfix, 35 | const base::Time& maxTimeA, 36 | const base::samples::RigidBodyState& startbody2Mls, 37 | const base::samples::RigidBodyState& endbody2Mls) 38 | { 39 | const std::string targetFile = getUnusedFilename(filePostfix); 40 | LOG_INFO_S << "Dumping planner state to: " << targetFile; 41 | std::ofstream output(targetFile, std::ios::binary | std::ios::out|std::ios::trunc); 42 | 43 | WRITE(planner.traversabilityConfig); 44 | WRITE(planner.mobility); 45 | WRITE(planner.splinePrimitiveConfig); 46 | WRITE(planner.plannerConfig); 47 | base::Pose tmp = startbody2Mls.getPose(); 48 | WRITE(tmp); 49 | tmp = endbody2Mls.getPose(); 50 | WRITE(tmp); 51 | double maxTimeD = maxTimeA.toSeconds(); 52 | 53 | WRITE(maxTimeD); 54 | 55 | boost::archive::binary_oarchive oa(output); 56 | oa << *(planner.env->getTraversabilityMap()); 57 | output.flush(); 58 | output.close(); 59 | } 60 | 61 | std::string ugv_nav4d::PlannerDump::getUnusedFilename(const std::string& filePostfix) const 62 | { 63 | std::string completeName; 64 | bool firstTime = true; 65 | do{ 66 | completeName = "ugv4d_dump_" + base::Time::now().toString(base::Time::Seconds, "%Y-%m-%d_%H%M%S") + "_" + filePostfix + ".bin"; 67 | 68 | if(!firstTime) 69 | usleep(10000); 70 | 71 | firstTime = false; 72 | } while(boost::filesystem::exists(completeName)); 73 | 74 | return completeName; 75 | } 76 | 77 | 78 | // void Planner::createDump(const std::string& filePostfix, const base::Time& maxTime, const base::samples::RigidBodyState& startbody2Mls, const base::samples::RigidBodyState& endbody2Mls) const 79 | // { 80 | // std::ofstream output(getUnusedFilename(filePostfix), std::ios::binary | std::ios::out|std::ios::trunc); 81 | // 82 | // WRITE(traversabilityConfig); 83 | // WRITE(mobility); 84 | // WRITE(splinePrimitiveConfig); 85 | // base::Pose tmp = startbody2Mls.getPose(); 86 | // WRITE(tmp); 87 | // tmp = endbody2Mls.getPose(); 88 | // WRITE(tmp); 89 | // double maxTimeD = maxTime.toSeconds(); 90 | // 91 | // WRITE(maxTimeD); 92 | // 93 | // boost::archive::binary_oarchive oa(output); 94 | // oa << env->getMlsMap(); 95 | // output.flush(); 96 | // output.close(); 97 | // } 98 | // 99 | // Planner::PLANNING_RESULT Planner::plan(const std::string& dumpName, std::vector< base::Trajectory >& resultTrajectory) 100 | // { 101 | // std::ifstream input(dumpName, std::ios::binary | std::ios::in); 102 | // 103 | // READ(traversabilityConfig); 104 | // READ(const_cast(mobility)); 105 | // READ(const_cast(splinePrimitiveConfig)); 106 | // base::Pose start; 107 | // READ(start); 108 | // base::Pose goal; 109 | // READ(goal); 110 | // double maxTime; 111 | // READ(maxTime); 112 | // 113 | // boost::shared_ptr mlsPtr(new MLSBase()); 114 | // boost::archive::binary_iarchive ia(input); 115 | // ia >> *(mlsPtr.get()); 116 | // 117 | // //resetup environment 118 | // env.reset(new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility)); 119 | // 120 | // base::samples::RigidBodyState startRBS; 121 | // startRBS.setPose(start); 122 | // base::samples::RigidBodyState goalRBS; 123 | // goalRBS.setPose(goal); 124 | // 125 | // return plan(base::Time::fromSeconds(maxTime), startRBS, goalRBS, resultTrajectory); 126 | // } 127 | -------------------------------------------------------------------------------- /src/Planner.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "EnvironmentXYZTheta.hpp" 5 | #include 6 | #include "PlannerConfig.hpp" 7 | 8 | #include 9 | 10 | class ARAPlanner; 11 | 12 | namespace ugv_nav4d 13 | { 14 | 15 | class PlannerDump; 16 | 17 | class Planner 18 | { 19 | protected: 20 | friend class PlannerDump; 21 | std::shared_ptr env; 22 | std::shared_ptr planner; 23 | 24 | const sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 25 | const Mobility mobility; 26 | traversability_generator3d::TraversabilityConfig traversabilityConfig; 27 | PlannerConfig plannerConfig; 28 | std::vector solutionIds; 29 | 30 | /**are buffered and reused for a more robust map generation */ 31 | std::vector previousStartPositions; 32 | 33 | public: 34 | enum PLANNING_RESULT { 35 | GOAL_INVALID, 36 | START_INVALID, 37 | NO_SOLUTION, /**< Happens if the planner runs out of time or the complete state space has been explored without a solution */ 38 | NO_MAP, 39 | INTERNAL_ERROR, 40 | FOUND_SOLUTION, 41 | }; 42 | 43 | Planner(const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig, 44 | const traversability_generator3d::TraversabilityConfig &traversabilityConfig, 45 | const Mobility& mobility, 46 | const PlannerConfig& plannerConfig); 47 | 48 | void updateMap(const traversability_generator3d::TravMap3d &map) 49 | { 50 | std::shared_ptr mapPtr = std::make_shared(map); 51 | 52 | if(!env) 53 | { 54 | env.reset(new EnvironmentXYZTheta(mapPtr, traversabilityConfig, splinePrimitiveConfig, mobility)); 55 | } 56 | else 57 | { 58 | env->updateMap(mapPtr); 59 | } 60 | } 61 | 62 | void enablePathStatistics(bool enable); 63 | 64 | std::vector getMotions() const; 65 | 66 | /** Plan a path from @p start to @p end. 67 | * 68 | * This will expand the map if it has not already been expanded. 69 | * 70 | * This function remembers the start positions from previous calls. Those positions will be used 71 | * (in addition to the current start position) to try to expand the map. Thus even if the current start 72 | * position is invalid (e.g. inside an obstacle) the map will still be expanded based on the previous start 73 | * positions. This feature improves the general robustness during planning and generally results in more 74 | * complete maps under real-world conditions. 75 | * 76 | * @param maxTime Maximum processor time to use. 77 | * @param startbody2Mls The start position of the body in mls coordinates. This should be the location of the body-frame. 78 | * The planner assumes that this location is config.distToGround meters above (!!!) the map. 79 | * The planner will transform this location to the ground frame using config.distToGround. 80 | * If this location is not exactly config.distToGround meters above the map, the planner will 81 | * fail to find the patch that the robot is standing on. 82 | * @param endbody2Mls The goal position of the body in mls coordinates. See @p startbody2Mls for more details. 83 | * @param resultTrajectory2D The resulting trajectory without z-coordinates (all z-values are set to zero). 84 | * This trajectory exists to avoid a bug in spline interpolation. In certain corner 85 | * cases (when the z change between two steps is much greater than the xy change) 86 | * the spline interpolator will fit an S-curve instead of a straight line between the 87 | * two patches. This results in an increase-decrease-increase pattern in xy-direction 88 | * resulting in a robot motion that stutters. Setting the z-axis to zero was choosen as 89 | * a fix because the trajectory follower (at the time of writing) ignores the z-axis anyway. 90 | * A ticket for this bug exists: https://git.hb.dfki.de/entern/ugv_nav4d/issues/1 91 | * @param resultTrajectory3D The resulting trajectory. Make sure to read the comment for @p resultTrajectory2D to understand 92 | * why this exists! 93 | * @param dumpOnError If true a planner dump will be written in case of error. This dump can be loaded and analyzed later 94 | * using the ugv_nav4d_replay tool. 95 | * @param dumpOnSuccess If true a planner dump will be written in case of successful planning. This dump can be loaded and analyzed later 96 | * using the ugv_nav4d_replay tool. 97 | * @return An enum indicating the planner state 98 | * */ 99 | PLANNING_RESULT plan(const base::Time& maxTime, const base::samples::RigidBodyState& start_pose, 100 | const base::samples::RigidBodyState& end_pose, std::vector& resultTrajectory2D, 101 | std::vector& resultTrajectory3D, bool dumpOnError = false, bool dumpOnSuccess = false); 102 | 103 | void setTravConfig(const traversability_generator3d::TraversabilityConfig& config); 104 | 105 | void setPlannerConfig(const PlannerConfig& config); 106 | 107 | const std::shared_ptr getTraversabilityMap() const; 108 | std::shared_ptr findTrajectoryOutOfObstacle(const Eigen::Vector3d& start, double theta, 109 | const Eigen::Affine3d& ground2Body, bool setZToZero); 110 | 111 | private: 112 | bool calculateGoal(Eigen::Vector3d& goal_translation, const double yaw) noexcept; 113 | bool tryGoal(const Eigen::Vector3d& translation, const double yaw) noexcept; 114 | 115 | }; 116 | 117 | } 118 | -------------------------------------------------------------------------------- /paper/paper.md: -------------------------------------------------------------------------------- 1 | --- 2 | title: 'ugv_nav4d: Advanced Multi-Surface Navigation for Unmanned Ground Vehicles Using 4D Path Planning Techniques' 3 | tags: 4 | - Multi-Surface Trajectory Planning 5 | - Global Path Planning 6 | - Unmanned Ground Vehicle Navigation 7 | - Mobile Robotics 8 | 9 | authors: 10 | - name: Arne Böckmann 11 | affiliation: 1 12 | - name: Janosch Machowinski 13 | affiliation: 1 14 | - name: Muhammad Haider Khan Lodhi 15 | orcid: 0009-0008-1199-3489 16 | affiliation: 2 17 | affiliations: 18 | - name: Cellumation GmbH 19 | index: 1 20 | - name: DFKI GmbH 21 | index: 2 22 | date: June 20, 2024 23 | bibliography: paper.bib 24 | 25 | --- 26 | 27 | # Summary 28 | 29 | The ugv_nav4d is a global path planner designed for terrestrial robots to navigate complex indoor and outdoor environments. To achieve this, ugv_nav4d uses a traversability map (TraversabilityMap3d) based on a multi-layered surface map (MLSMap) [@mlsmaps; @slammaps], enabling planning in multi-surface environments. The path is built by selecting motion primitives tailored to the robot's mechanical features, ensuring compatibility with its motion capabilities. 30 | 31 | ![Planned trajectory in a multi-storey environment.](figures/parking_deck.png){width="300pt"} 32 | 33 | # Statement Of Need 34 | Accurate ground surface representation is crucial for ground-based robots in complex terrains. The ROS2 Navigation Stack (nav2) [@macenski2020nav2], which uses voxel maps for 3D navigation, often loses detail and accuracy, especially in multi-storey environments, due to its discrete voxelization and separate costmaps for each floor. 35 | 36 | We propose ugv_nav4d, a path planner that enhances environmental representation with Multi-Layered Surface Maps (MLS) [@mlsmaps] and a 3D Traversability Map [@slammaps]. Ugv_nav4d avoids the "stepping" effect of voxel maps by using a continuous grid and detailed vertical information, providing smoother and more accurate terrain modelling. 37 | 38 | Unlike nav2, ugv_nav4d simplifies planning with a single TraversabilityMap3D, which contains detailed ground surface data, offering a superior alternative to nav2’s 3D costmaps. For users, MLS maps provide a smoother, more realistic view of terrain compared to the blocky voxel maps, enhancing navigation and decision-making in complex environments. 39 | 40 | # Software Components 41 | The core software components of the planner are 42 | 43 | - EnvironmentXYZTheta 44 | - PathPlanner 45 | - PreComputedMotions 46 | 47 | ### EnvironmentXYZTheta 48 | The core of ugv_nav4d is based on SBPL (Search-Based Planning Library) [@sbpl]. The EnvironmentXYZTheta implements all interfaces needed by SBPL to enable ARA* based planning. The environment in SBPL is a state space which connects states with associated transition costs. A state is defined by the position (x,y,z) and orientation (yaw) of the robot. The EnvironmentXYZTheta uses a TraversabilityGenerator3d [@travgen3d] to generate a TraversabilityMap3d from a MLSMap, which classifies the MLSMap patches into traversable, non-traversable, and unknown terrain and stores metadata of the ground surface (e.g., slope of the patch, supporting plane, etc.). 49 | 50 | ![The Moon Crater and its TraversabilityMap3d in the Space Hall at the RIC, DFKI [@roboticsElab]\label{fig:crater}.](figures/trav_map_and_space_hall.png){width="300pt"} 51 | 52 | ### PathPlanner 53 | The search for the shortest path in ugv_nav4d is based on the SBPL-provided ARA* planner. It builds a state space of future robot states and uses a time heuristic to compute the shortest path on the TraversabilityMap3D. Robot and terrain-specific information, including robot dimensions and orientations, collision checks with obstacles, terrain steepness, and motion primitives are incorporated as additional costs in the search for the successive states. The planner generates a complete trajectory for the planned path, which can then be executed by a trajectory follower [@trajectoryfollower]. 54 | 55 | ![The planned trajectory (yellow) using the robot [Hunter-SE](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/hunterse) in the final demostration of the project [KIMMI-SF](https://robotik.dfki-bremen.de/de/forschung/projekte/kimmi-sf).](figures/motion_planning_hunter.png){width="200pt" height="300pt"} 56 | 57 | ### PreComputedMotions 58 | Motion primitives are pre-defined movements stored as spline trajectories, representing feasible robot motions from a given pose (x, y, z, yaw). The planner uses these primitives to find successor states, ensuring paths are traversable and collision-free. Each motion has a base cost for flat surfaces, with additional penalties for factors like steepness. Primitives fall into four categories: Forward, Backward, Lateral, and Point-Turn. These primitives can be tailored for different robot types: Ackermann drive robots use forward and backward primitives, differential drive robots can use point-turn primitives, and omni-directional robots utilize all four types. Point-turn primitives are unique as they don't use splines, unlike the others generated by the SbplSplineMotionPrimitives library [@sbplsplineprimitives]. 59 | 60 | ![A pool of forward motion primitives by the PreComputedMotions module.](figures/motion_primitives.png){width="200pt" height="300pt"} 61 | 62 | # Debugging 63 | 64 | ### PlannerGui 65 | A GUI for ugv_nav4d allows debugging and testing by experimenting with planner parameters on a static map. The PlannerGui can load point clouds from ply or serialized MLS maps, using left and right clicks to set start and end locations. Errors can be logged to a file and ugv_nav4d_replay can be used to analyze the state and execute planning in a controlled environment. 66 | 67 | ![GUI for debugging and testing of ugv_nav4d.](figures/debug_gui.png){width="200pt" height="300pt"} 68 | 69 | # Field Tests 70 | ugv_nav4d has been used extensively in research projects for almost a decade. It has been utilized for autonomous navigation in several projects, including [Entern](https://robotik.dfki-bremen.de/de/forschung/projekte/entern), [ANT](https://robotik.dfki-bremen.de/de/forschung/projekte/ant), [VIPE](https://robotik.dfki-bremen.de/de/forschung/projekte/vipe), [KIMMI-SF](https://robotik.dfki-bremen.de/de/forschung/projekte/kimmi-sf), [HiSE](https://robotik.dfki-bremen.de/de/forschung/projekte/hise), [PerSim](https://robotik.dfki-bremen.de/de/forschung/projekte/persim), and [CoRobX](https://robotik.dfki-bremen.de/de/forschung/projekte/corob-x). As a versatile planner, it has also been used for various terrestrial robots, such as [Artemis](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/artemis), [Coyote-III](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/coyote-iii), [Crex](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/crex), [Charlie](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/charlie), [Hunter-SE](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/hunterse), [SherpaTT](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/sherpatt), and [Asguard-IV](https://robotik.dfki-bremen.de/de/forschung/robotersysteme/asguard-iv). 71 | 72 | # Acknowledgements 73 | The ugv_nav4d library was initiated and is currently developed at the Robotics Innovation Center of the German Research Center for Artificial Intelligence (DFKI) in Bremen, together with the Robotics Group of the University of Bremen. The development was started in the scope of the Entern project (50RA1406), which has been funded by the German Aerospace Center (DLR) with funds from the German Federal Ministry for Economic Affairs and Climate Action (BMWK). 74 | 75 | We would also like to acknowledge the contributors of the ugv_nav4d repository, including the developers visible on the project's GitHub contributors page, for their valuable efforts and dedication. 76 | 77 | # References -------------------------------------------------------------------------------- /src/test/test_PreComputedMotions.cpp: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_MODULE PreComputedMotionsModule 2 | #include 3 | 4 | #include "ugv_nav4d/PreComputedMotions.hpp" 5 | #include 6 | 7 | using namespace ugv_nav4d; 8 | 9 | BOOST_AUTO_TEST_CASE(initialization) { 10 | 11 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 12 | Mobility mobilityConfig; 13 | 14 | PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); 15 | delete motions; 16 | } 17 | 18 | BOOST_AUTO_TEST_CASE(calculate_curvature_from_radius) { 19 | 20 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 21 | Mobility mobilityConfig; 22 | 23 | PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); 24 | double curvature = motions->calculateCurvatureFromRadius(2.0); 25 | BOOST_CHECK_CLOSE_FRACTION(curvature, 0.5, 0.001); 26 | 27 | delete motions; 28 | } 29 | 30 | BOOST_AUTO_TEST_CASE(generate_motions) { 31 | 32 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 33 | Mobility mobilityConfig; 34 | mobilityConfig.minTurningRadius = 1.0; 35 | 36 | PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); 37 | motions->computeMotions(splinePrimitiveConfig.gridSize, splinePrimitiveConfig.gridSize); 38 | delete motions; 39 | } 40 | 41 | BOOST_AUTO_TEST_CASE(check_motions) { 42 | 43 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 44 | Mobility mobilityConfig; 45 | mobilityConfig.minTurningRadius = 1.0; 46 | 47 | PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); 48 | motions->computeMotions(splinePrimitiveConfig.gridSize, splinePrimitiveConfig.gridSize); 49 | 50 | //We generate primitives for 16 start angles. 51 | for (int i = 1; i <= 16; ++i) { 52 | DiscreteTheta theta(i,16); 53 | std::vector motions_start = motions->getMotionForStartTheta(theta); 54 | BOOST_REQUIRE(motions_start.size() > 0); 55 | for (const auto& motion : motions_start) { 56 | BOOST_REQUIRE(motion.baseCost > 0); 57 | BOOST_REQUIRE(motion.costMultiplier > 0); 58 | BOOST_REQUIRE(motion.costScaleFactor > 0); 59 | 60 | switch (motion.type){ 61 | case Motion::MOV_FORWARD: 62 | BOOST_CHECK_GT(motion.fullSplineSamples.size(), 0); 63 | BOOST_CHECK_GT(motion.intermediateStepsTravMap.size(), 0); 64 | BOOST_CHECK_GT(motion.intermediateStepsObstMap.size(), 0); 65 | break; 66 | case Motion::MOV_BACKWARD: 67 | BOOST_CHECK_GT(motion.fullSplineSamples.size(), 0); 68 | BOOST_CHECK_GT(motion.intermediateStepsTravMap.size(), 0); 69 | BOOST_CHECK_GT(motion.intermediateStepsObstMap.size(), 0); 70 | break; 71 | case Motion::MOV_LATERAL: 72 | BOOST_CHECK_GT(motion.fullSplineSamples.size(), 0); 73 | BOOST_CHECK_GT(motion.intermediateStepsTravMap.size(), 0); 74 | BOOST_CHECK_GT(motion.intermediateStepsObstMap.size(), 0); 75 | BOOST_CHECK_EQUAL(motion.startTheta.getRadian(), 76 | motion.endTheta.getRadian()); 77 | break; 78 | case Motion:: MOV_POINTTURN: 79 | BOOST_CHECK_EQUAL(motion.fullSplineSamples.size(), 0); 80 | BOOST_CHECK_NE(motion.startTheta.getRadian(), motion.endTheta.getRadian()); 81 | break; 82 | default: 83 | throw std::runtime_error("test_PreComputedMotions: Invalid motion type detectd!"); 84 | break; 85 | 86 | } 87 | 88 | //Point-turns do not have any splines 89 | if (motion.type != Motion::MOV_POINTTURN){ 90 | for (const auto& cellWithPoses : motion.fullSplineSamples){ 91 | BOOST_CHECK_GT(cellWithPoses.poses.size(), 0); 92 | } 93 | 94 | base::Angle splineStartAngle = base::Angle::fromRad(motion.fullSplineSamples.front().poses.front().orientation); 95 | base::Angle splineEndAngle = base::Angle::fromRad(motion.fullSplineSamples.back().poses.back().orientation); 96 | 97 | base::Angle motionStartAngle = base::Angle::fromRad(motion.startTheta.getRadian()); 98 | base::Angle motionEndAngle = base::Angle::fromRad(motion.endTheta.getRadian()); 99 | 100 | if (motion.type == Motion::MOV_FORWARD){ 101 | BOOST_CHECK_CLOSE_FRACTION(splineStartAngle.getRad(), motionStartAngle.getRad(), 0.01); 102 | BOOST_CHECK_CLOSE_FRACTION(splineEndAngle.getRad(), motionEndAngle.getRad(), 0.01); 103 | } 104 | 105 | //Backward motions have a 180 degree angle diff between spline and motion 106 | else if (motion.type == Motion::MOV_BACKWARD){ 107 | BOOST_CHECK_CLOSE_FRACTION(std::abs((splineStartAngle - motionStartAngle).getRad()), 3.142, 0.01); 108 | BOOST_CHECK_CLOSE_FRACTION(std::abs((splineEndAngle - motionEndAngle).getRad()), 3.142, 0.01); 109 | } 110 | 111 | //Lateral motions have a 90 degree angle diff between spline and motion (robot maintains heading) 112 | else if (motion.type == Motion::MOV_LATERAL){ 113 | BOOST_CHECK_CLOSE_FRACTION(std::abs((splineStartAngle - motionStartAngle).getRad()), 1.57, 0.01); 114 | BOOST_CHECK_CLOSE_FRACTION(std::abs((splineEndAngle - motionEndAngle).getRad()), 1.57, 0.01); 115 | } 116 | 117 | const base::Pose2D& splineFinalPosition = motion.fullSplineSamples.back().poses.back(); 118 | const base::Pose2D& travMapFinalPosition = motion.intermediateStepsTravMap.back().pose; 119 | const base::Pose2D& obstMapFinalPosition = motion.intermediateStepsObstMap.back().pose; 120 | 121 | //Check if end position of full spline and sampled spline are same 122 | BOOST_REQUIRE(splineFinalPosition.position.isApprox(travMapFinalPosition.position, 1e-6)); 123 | BOOST_REQUIRE(splineFinalPosition.position.isApprox(obstMapFinalPosition.position, 1e-6)); 124 | } 125 | } 126 | } 127 | 128 | delete motions; 129 | } 130 | 131 | BOOST_AUTO_TEST_CASE(calculate_cost) { 132 | 133 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 134 | Mobility mobilityConfig; 135 | 136 | PreComputedMotions* motions = new PreComputedMotions(splinePrimitiveConfig,mobilityConfig); 137 | motions->computeMotions(splinePrimitiveConfig.gridSize, splinePrimitiveConfig.gridSize); 138 | 139 | //Start angle 0 degrees (First set of primitives from 16 sets) 140 | DiscreteTheta theta(1,16); 141 | std::vector motions_start = motions->getMotionForStartTheta(theta); 142 | BOOST_REQUIRE(motions_start.size() > 0); 143 | BOOST_CHECK_THROW(motions_start[0].calculateCost(0,0,0,0,0), std::runtime_error); 144 | 145 | //Translation 146 | BOOST_CHECK_GT(motions_start[0].calculateCost(1,0,0.1,0.1,1), 0); 147 | //Rotation 148 | BOOST_CHECK_GT(motions_start[0].calculateCost(0,1,0.1,0.1,1), 0); 149 | //Translation and rotation 150 | BOOST_CHECK_GT(motions_start[0].calculateCost(1,1,0.1,0.1,1), 0); 151 | 152 | delete motions; 153 | } -------------------------------------------------------------------------------- /src/test/test_EnvironmentXYZTheta.cpp: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_MODULE EnvironmentXYZThetaTestModule 2 | #include 3 | #include 4 | #include 5 | 6 | #include "ugv_nav4d/EnvironmentXYZTheta.hpp" 7 | 8 | using namespace ugv_nav4d; 9 | using namespace maps::grid; 10 | 11 | std::vector startPositions; 12 | std::vector goalPositions; 13 | 14 | struct EnvironmentXYZThetaTest { 15 | EnvironmentXYZThetaTest(){} 16 | ~EnvironmentXYZThetaTest(){} 17 | 18 | 19 | EnvironmentXYZTheta* environment; 20 | Mobility mobility; 21 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 22 | traversability_generator3d::TraversabilityConfig traversabilityConfig; 23 | bool map_loaded = false; 24 | }; 25 | 26 | BOOST_FIXTURE_TEST_CASE(travmap_resolution_not_equal_to_mls_resolution, EnvironmentXYZThetaTest) { 27 | Vector2d res(0.3, 0.3); 28 | Vector2ui numCells(10, 10); 29 | 30 | MLSConfig mls_config; 31 | mls_config.gapSize = 0.1; 32 | mls_config.updateModel = MLSConfig::SLOPE; 33 | MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); 34 | 35 | /** Translate the local frame (offset) **/ 36 | mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; 37 | 38 | Eigen::Vector2d max = 0.5 * mls_o.getSize(); 39 | Eigen::Vector2d min = -0.5 * mls_o.getSize(); 40 | for (double x = min.x(); x < max.x(); x += 0.00625) 41 | { 42 | //double cs = std::cos(x * M_PI/2.5); 43 | for (double y = min.y(); y < max.y(); y += 0.00625) 44 | { 45 | //double sn = std::sin(y * M_PI/2.5); 46 | mls_o.mergePoint(Eigen::Vector3d(x, y, 0)); 47 | } 48 | } 49 | 50 | std::shared_ptr mlsPtr = std::make_shared(mls_o); 51 | traversabilityConfig.gridResolution = 0.5; 52 | splinePrimitiveConfig.gridSize = 0.5; 53 | environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); 54 | std::vector startPositions; 55 | startPositions.emplace_back(Eigen::Vector3d(0,0,0)); 56 | environment->expandMap(startPositions); 57 | BOOST_CHECK_EQUAL(environment->getTravGen().getNumNodes(), 1); 58 | delete environment; 59 | 60 | traversabilityConfig.gridResolution = 0.1; 61 | splinePrimitiveConfig.gridSize = 0.1; 62 | environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); 63 | environment->expandMap(startPositions); 64 | BOOST_CHECK_EQUAL(environment->getTravGen().getNumNodes(), 9); 65 | } 66 | 67 | BOOST_FIXTURE_TEST_CASE(travmap_resolution_equal_to_mls_resolution, EnvironmentXYZThetaTest) { 68 | Vector2d res(0.3, 0.3); 69 | Vector2ui numCells(10, 10); 70 | 71 | MLSConfig mls_config; 72 | mls_config.gapSize = 0.1; 73 | mls_config.updateModel = MLSConfig::SLOPE; 74 | MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); 75 | 76 | /** Translate the local frame (offset) **/ 77 | mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; 78 | 79 | Eigen::Vector2d max = 0.5 * mls_o.getSize(); 80 | Eigen::Vector2d min = -0.5 * mls_o.getSize(); 81 | 82 | for (double x = min.x(); x < max.x(); x += 0.00625) 83 | { 84 | //double cs = std::cos(x * M_PI/2.5); 85 | for (double y = min.y(); y < max.y(); y += 0.00625) 86 | { 87 | //double sn = std::sin(y * M_PI/2.5); 88 | mls_o.mergePoint(Eigen::Vector3d(x, y, 0)); 89 | } 90 | } 91 | 92 | std::shared_ptr mlsPtr = std::make_shared(mls_o); 93 | traversabilityConfig.gridResolution = 0.3; 94 | splinePrimitiveConfig.gridSize = 0.3; 95 | 96 | environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); 97 | std::vector startPositions; 98 | startPositions.emplace_back(Eigen::Vector3d(0,0,0)); 99 | environment->expandMap(startPositions); 100 | BOOST_CHECK_EQUAL(environment->getTravGen().getNumNodes(), numCells.x()* numCells.y()); 101 | } 102 | 103 | BOOST_FIXTURE_TEST_CASE(check_travmap, EnvironmentXYZThetaTest) { 104 | Vector2d res(0.3, 0.3); 105 | Vector2ui numCells(10, 10); 106 | 107 | MLSConfig mls_config; 108 | mls_config.gapSize = 0.1; 109 | mls_config.updateModel = MLSConfig::SLOPE; 110 | MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); 111 | 112 | /** Translate the local frame (offset) **/ 113 | mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; 114 | 115 | Eigen::Vector2d max = 0.5 * mls_o.getSize(); 116 | Eigen::Vector2d min = -0.5 * mls_o.getSize(); 117 | 118 | for (double x = min.x(); x < max.x(); x += 0.00625) 119 | { 120 | for (double y = min.y(); y < max.y(); y += 0.00625) 121 | { 122 | double z = 0; 123 | if ((x >= 0.6 && x < 0.9) && (y >= 0.6 && y < 0.9)){ 124 | z = 0.3; 125 | } 126 | mls_o.mergePoint(Eigen::Vector3d(x, y, z)); 127 | } 128 | } 129 | 130 | std::shared_ptr mlsPtr = std::make_shared(mls_o); 131 | traversabilityConfig.gridResolution = 0.3; 132 | splinePrimitiveConfig.gridSize = 0.3; 133 | 134 | environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); 135 | std::vector startPositions; 136 | startPositions.emplace_back(Eigen::Vector3d(0,0,0)); 137 | environment->expandMap(startPositions); 138 | 139 | Eigen::Vector3d positionFron{0.9, 0.3, 0}; 140 | maps::grid::Index idxFrontierNode; 141 | environment->getTravGen().getTraversabilityMap()->toGrid(positionFron, idxFrontierNode); 142 | auto frontier = environment->getTravGen().findMatchingTraversabilityPatchAt(idxFrontierNode,0); 143 | BOOST_CHECK_EQUAL(frontier->getType(), ::maps::grid::TraversabilityNodeBase::FRONTIER); 144 | 145 | Eigen::Vector3d positionTrav{0.3, 0.3, 0}; 146 | maps::grid::Index idxTraversableNode; 147 | environment->getTravGen().getTraversabilityMap()->toGrid(positionTrav, idxTraversableNode); 148 | auto traversable = environment->getTravGen().findMatchingTraversabilityPatchAt(idxTraversableNode,0); 149 | BOOST_CHECK_EQUAL(traversable->getType(), ::maps::grid::TraversabilityNodeBase::TRAVERSABLE); 150 | 151 | Eigen::Vector3d positionObs{0.65, 0.65, 0}; 152 | maps::grid::Index idxObstacleNode; 153 | environment->getTravGen().getTraversabilityMap()->toGrid(positionObs, idxObstacleNode); 154 | auto &trList(environment->getTravGen().getTraversabilityMap()->at(idxObstacleNode)); 155 | for(auto *snode : trList) 156 | { 157 | BOOST_CHECK_EQUAL(snode->getType(), ::maps::grid::TraversabilityNodeBase::OBSTACLE); 158 | } 159 | } 160 | 161 | BOOST_FIXTURE_TEST_CASE(check_stepheight, EnvironmentXYZThetaTest) { 162 | Vector2d res(0.3, 0.3); 163 | Vector2ui numCells(10, 10); 164 | 165 | MLSConfig mls_config; 166 | mls_config.gapSize = 0.1; 167 | mls_config.updateModel = MLSConfig::SLOPE; 168 | MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); 169 | 170 | /** Translate the local frame (offset) **/ 171 | mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; 172 | 173 | Eigen::Vector2d max = 0.5 * mls_o.getSize(); 174 | Eigen::Vector2d min = -0.5 * mls_o.getSize(); 175 | 176 | for (double x = min.x(); x < max.x(); x += 0.00625) 177 | { 178 | for (double y = min.y(); y < max.y(); y += 0.00625) 179 | { 180 | double z = 0; 181 | if ((x >= 0 && x < 0.3) && (y >= 0 && y < 0.3)){ 182 | z = 0.1; 183 | } 184 | mls_o.mergePoint(Eigen::Vector3d(x, y, z)); 185 | } 186 | } 187 | 188 | std::shared_ptr mlsPtr = std::make_shared(mls_o); 189 | traversabilityConfig.gridResolution = 0.3; 190 | splinePrimitiveConfig.gridSize = 0.3; 191 | 192 | environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); 193 | std::vector startPositions; 194 | startPositions.emplace_back(Eigen::Vector3d(0,0,0)); 195 | environment->expandMap(startPositions); 196 | 197 | Eigen::Vector3d positionObs{0.25, 0.25, 0}; 198 | maps::grid::Index idxObstacleNode; 199 | 200 | environment->getTravGen().getTraversabilityMap()->toGrid(positionObs, idxObstacleNode); 201 | for(auto *snode : environment->getTravGen().getTraversabilityMap()->at(idxObstacleNode)) 202 | { 203 | BOOST_CHECK_EQUAL(snode->getType(), ::maps::grid::TraversabilityNodeBase::OBSTACLE); 204 | } 205 | 206 | delete environment; 207 | 208 | traversabilityConfig.maxStepHeight = 0.2; 209 | environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); 210 | environment->expandMap(startPositions); 211 | 212 | environment->getTravGen().getTraversabilityMap()->toGrid(positionObs, idxObstacleNode); 213 | for(auto *snode : environment->getTravGen().getTraversabilityMap()->at(idxObstacleNode)) 214 | { 215 | BOOST_CHECK_EQUAL(snode->getType(), ::maps::grid::TraversabilityNodeBase::TRAVERSABLE); 216 | } 217 | } 218 | 219 | BOOST_FIXTURE_TEST_CASE(set_start_and_goal, EnvironmentXYZThetaTest) { 220 | Vector2d res(0.3, 0.3); 221 | Vector2ui numCells(100, 100); 222 | 223 | MLSConfig mls_config; 224 | mls_config.gapSize = 0.1; 225 | mls_config.updateModel = MLSConfig::SLOPE; 226 | MLSMapSloped mls_o = MLSMapSloped(numCells, res, mls_config); 227 | 228 | /** Translate the local frame (offset) **/ 229 | mls_o.getLocalFrame().translation() << 0.5*mls_o.getSize(), 0; 230 | 231 | Eigen::Vector2d max = 0.5 * mls_o.getSize(); 232 | Eigen::Vector2d min = -0.5 * mls_o.getSize(); 233 | 234 | for (double x = min.x(); x < max.x(); x += 0.00625) 235 | { 236 | for (double y = min.y(); y < max.y(); y += 0.00625) 237 | { 238 | mls_o.mergePoint(Eigen::Vector3d(x, y, 0)); 239 | } 240 | } 241 | 242 | std::shared_ptr mlsPtr = std::make_shared(mls_o); 243 | traversabilityConfig.gridResolution = 0.3; 244 | splinePrimitiveConfig.gridSize = 0.3; 245 | 246 | environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility); 247 | std::vector startPositions; 248 | startPositions.emplace_back(Eigen::Vector3d(0,0,0)); 249 | environment->expandMap(startPositions); 250 | 251 | Eigen::Vector3d startPos{0,0,0}; 252 | Eigen::Vector3d goalPos{12, 12, 0}; 253 | 254 | environment->setStart(startPos,0); 255 | environment->setGoal(goalPos,0); 256 | } 257 | -------------------------------------------------------------------------------- /src/Planner.cpp: -------------------------------------------------------------------------------- 1 | #include "Planner.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "PlannerDump.hpp" 7 | #include 8 | #include 9 | #include 10 | #include "Logger.hpp" 11 | #include 12 | #include 13 | 14 | #ifdef ENABLE_DEBUG_DRAWINGS 15 | #include 16 | #include 17 | #endif 18 | 19 | using namespace maps::grid; 20 | using trajectory_follower::SubTrajectory; 21 | 22 | namespace ugv_nav4d 23 | { 24 | 25 | 26 | Planner::Planner(const sbpl_spline_primitives::SplinePrimitivesConfig& primitiveConfig, const traversability_generator3d::TraversabilityConfig& traversabilityConfig, 27 | const Mobility& mobility, const PlannerConfig& plannerConfig) : 28 | splinePrimitiveConfig(primitiveConfig), 29 | mobility(mobility), 30 | plannerConfig(plannerConfig) 31 | { 32 | setTravConfig(traversabilityConfig); 33 | } 34 | 35 | void Planner::enablePathStatistics(bool enable){ 36 | if (env){ 37 | env->enablePathStatistics(enable); 38 | } 39 | } 40 | 41 | bool Planner::calculateGoal(Eigen::Vector3d& goal_translation, const double yaw) noexcept 42 | { 43 | if (tryGoal(goal_translation, yaw)){ 44 | return true; 45 | } 46 | 47 | traversability_generator3d::TravGenNode* travNode = env->findMatchingTraversabilityPatchAt(goal_translation); 48 | if (!travNode){ 49 | return false; 50 | } 51 | 52 | auto trMap = env->getTraversabilityMap(); 53 | auto pos1 = travNode->getPosition(*trMap); 54 | 55 | std::deque candidates; 56 | std::unordered_set visited; 57 | 58 | candidates.push_back(travNode); 59 | visited.insert(travNode); 60 | 61 | while(!candidates.empty()) 62 | { 63 | auto *node = candidates.front(); 64 | candidates.pop_front(); 65 | 66 | for(auto *n : node->getConnections()){ 67 | if (visited.count(n)) continue; // Skip visited 68 | 69 | auto pos2 = n->getPosition(*trMap); 70 | if ((pos1 - pos2).norm() > mobility.searchRadius){ 71 | continue; 72 | } 73 | 74 | if (tryGoal(pos2, yaw)){ 75 | goal_translation = pos2; 76 | LOG_INFO_S << "Estimated Goal Position: " << goal_translation.transpose(); 77 | return true; 78 | } 79 | 80 | candidates.push_back(n); 81 | visited.insert(n); 82 | } 83 | } 84 | 85 | return false; // Add this to cover all control paths 86 | } 87 | 88 | bool Planner::tryGoal(const Eigen::Vector3d& translation, const double yaw) noexcept 89 | { 90 | try 91 | { 92 | env->setGoal(translation, yaw); 93 | } 94 | catch(const std::runtime_error& ex) 95 | { 96 | LOG_ERROR_S << "Caught exception while setting goal pose:" << ex.what(); 97 | return false; 98 | } 99 | return true; 100 | } 101 | 102 | Planner::PLANNING_RESULT Planner::plan(const base::Time& maxTime, const base::samples::RigidBodyState& start_pose, 103 | const base::samples::RigidBodyState& end_pose, 104 | std::vector& resultTrajectory2D, 105 | std::vector& resultTrajectory3D, 106 | bool dumpOnError, bool dumpOnSuccess) 107 | { 108 | 109 | LOG_DEBUG_S << "Planning with " << plannerConfig.numThreads << " threads"; 110 | omp_set_num_threads(plannerConfig.numThreads); 111 | #ifdef ENABLE_DEBUG_DRAWINGS 112 | V3DD::CLEAR_DRAWING("ugv_nav4d_successors"); 113 | #endif 114 | if(!env) 115 | { 116 | LOG_ERROR_S << "Planner::plan : Error : No map was set"; 117 | return NO_MAP; 118 | } 119 | 120 | resultTrajectory2D.clear(); 121 | resultTrajectory3D.clear(); 122 | env->clear(); 123 | 124 | if(!planner) 125 | planner.reset(new ARAPlanner(env.get(), true)); 126 | 127 | 128 | Eigen::Affine3d ground2Body(Eigen::Affine3d::Identity()); 129 | ground2Body.translation() = Eigen::Vector3d(0, 0, -traversabilityConfig.distToGround); 130 | 131 | base::samples::RigidBodyState startbody2Mls = start_pose; 132 | base::samples::RigidBodyState endbody2Mls = end_pose; 133 | 134 | startbody2Mls.setTransform(startbody2Mls.getTransform()); 135 | endbody2Mls.setTransform(endbody2Mls.getTransform()); 136 | 137 | LOG_DEBUG_S << "start_pose position (raw): " << start_pose.position.transpose(); 138 | LOG_DEBUG_S << "end_pose position (raw): " << end_pose.position.transpose(); 139 | 140 | const Eigen::Affine3d startGround2Mls(startbody2Mls.getTransform() * ground2Body); 141 | const Eigen::Affine3d endGround2Mls(endbody2Mls.getTransform() *ground2Body); 142 | 143 | startbody2Mls.setTransform(startGround2Mls); 144 | endbody2Mls.setTransform(endGround2Mls); 145 | 146 | try 147 | { 148 | env->setStart(startGround2Mls.translation(), base::getYaw(Eigen::Quaterniond(startGround2Mls.linear()))); 149 | } 150 | catch(const ugv_nav4d::ObstacleCheckFailed& ex) 151 | { 152 | LOG_ERROR_S << "Caught exception while setting start pose:" << ex.what(); 153 | if(dumpOnError) 154 | PlannerDump dump(*this, "start_inside_obstacle", maxTime, startbody2Mls, endbody2Mls); 155 | return START_INVALID; 156 | } 157 | catch(const std::runtime_error& ex) 158 | { 159 | LOG_ERROR_S << "Caught exception while setting start pose:" << ex.what(); 160 | if(dumpOnError) 161 | PlannerDump dump(*this, "bad_start", maxTime, startbody2Mls, endbody2Mls); 162 | return START_INVALID; 163 | } 164 | 165 | Eigen::Vector3d start_translation = startGround2Mls.translation(); 166 | Eigen::Vector3d goal_translation = endGround2Mls.translation(); 167 | 168 | if(!calculateGoal(goal_translation, base::getYaw(Eigen::Quaterniond(endGround2Mls.linear())))) { 169 | if(dumpOnError) { 170 | PlannerDump dump(*this, "bad_goal", maxTime, startbody2Mls, endbody2Mls); 171 | } 172 | return GOAL_INVALID; 173 | } 174 | 175 | //this has to happen after env->setStart and env->setGoal because those methods initialize the 176 | //StateID2IndexMapping which is accessed inside force_planning_from_scratch_and_free_memory(). 177 | try 178 | { 179 | planner->force_planning_from_scratch_and_free_memory(); 180 | planner->set_search_mode(plannerConfig.searchUntilFirstSolution); 181 | } 182 | catch(const SBPL_Exception& ex) 183 | { 184 | LOG_ERROR_S << "Caught SBPL exception: " << ex.what(); 185 | return NO_SOLUTION; 186 | } 187 | 188 | 189 | MDPConfig mdp_cfg; 190 | 191 | if (!env->InitializeMDPCfg(&mdp_cfg)) { 192 | LOG_ERROR_S << "InitializeMDPCfg failed, start and goal id cannot be requested yet"; 193 | return INTERNAL_ERROR; 194 | } 195 | if (planner->set_start(mdp_cfg.startstateid) == 0) { 196 | LOG_ERROR_S << "Failed to set start state"; 197 | return INTERNAL_ERROR; 198 | } 199 | if (planner->set_goal(mdp_cfg.goalstateid) == 0) { 200 | LOG_ERROR_S << "Failed to set goal state"; 201 | return INTERNAL_ERROR; 202 | } 203 | 204 | try 205 | { 206 | LOG_DEBUG_S << "Initial Epsilon: " << plannerConfig.initialEpsilon << ", steps: " << plannerConfig.epsilonSteps; 207 | planner->set_eps_step(plannerConfig.epsilonSteps); 208 | planner->set_initialsolution_eps(plannerConfig.initialEpsilon); 209 | 210 | solutionIds.clear(); 211 | if(!planner->replan(maxTime.toSeconds(), &solutionIds)) 212 | { 213 | LOG_DEBUG_S << "Number of state space expands: " << planner->get_n_expands(); 214 | if(dumpOnError) 215 | PlannerDump dump(*this, "no_solution", maxTime, startbody2Mls, endbody2Mls); 216 | return NO_SOLUTION; 217 | } 218 | 219 | LOG_DEBUG_S << "num expands: " << planner->get_n_expands(); 220 | LOG_DEBUG_S << "Epsilon is " << planner->get_final_epsilon(); 221 | 222 | std::vector stats; 223 | 224 | planner->get_search_stats(&stats); 225 | env->getTrajectory(solutionIds, resultTrajectory2D, true, start_translation, goal_translation, end_pose.getYaw(), ground2Body); 226 | env->getTrajectory(solutionIds, resultTrajectory3D, false, start_translation, goal_translation,end_pose.getYaw(), ground2Body); 227 | } 228 | catch(const SBPL_Exception& ex) 229 | { 230 | LOG_ERROR_S << "Caught sbpl exception: " << ex.what(); 231 | if(dumpOnError) 232 | PlannerDump dump(*this, "no_solution", maxTime, startbody2Mls, endbody2Mls); 233 | return NO_SOLUTION; 234 | } 235 | 236 | if(dumpOnSuccess) 237 | PlannerDump dump(*this, "success", maxTime, startbody2Mls, endbody2Mls); 238 | 239 | return FOUND_SOLUTION; 240 | } 241 | 242 | std::vector< Motion > Planner::getMotions() const 243 | { 244 | return env->getMotions(solutionIds); 245 | } 246 | 247 | const std::shared_ptr Planner::getTraversabilityMap() const 248 | { 249 | return env->getTraversabilityMap(); 250 | } 251 | 252 | std::shared_ptr Planner::findTrajectoryOutOfObstacle(const Eigen::Vector3d& start, 253 | double theta, 254 | const Eigen::Affine3d& ground2Body, 255 | bool setZToZero){ 256 | if(env){ 257 | try{ 258 | return env->findTrajectoryOutOfObstacle(start, theta, ground2Body, setZToZero); 259 | } 260 | catch (const std::exception& e){ 261 | LOG_ERROR_S << "Caught exception when finding trajectory out of obstacle: " << e.what(); 262 | return nullptr; 263 | } 264 | } 265 | else { 266 | return nullptr; 267 | } 268 | } 269 | 270 | void Planner::setTravConfig(const traversability_generator3d::TraversabilityConfig& config) 271 | { 272 | if(config.gridResolution != splinePrimitiveConfig.gridSize){ 273 | LOG_ERROR_S << "Planner::Planner : Configuration error, grid resolution of Primitives and TraversabilityGenerator3d differ"; 274 | throw std::runtime_error("Planner::Planner : Configuration error, grid resolution of Primitives and TraversabilityGenerator3d differ"); 275 | } 276 | traversabilityConfig = config; 277 | if(env){ 278 | env->setTravConfig(config); 279 | } 280 | } 281 | 282 | void Planner::setPlannerConfig(const PlannerConfig& config) 283 | { 284 | plannerConfig = config; 285 | } 286 | 287 | } 288 | -------------------------------------------------------------------------------- /src/EnvironmentXYZTheta.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #undef DEBUG //sbpl defines DEBUG 0 but the word debug is also used in base-logging which is included from TraversabilityGenerator3d 5 | #include 6 | #include 7 | #include 8 | #include "DiscreteTheta.hpp" 9 | #include "PreComputedMotions.hpp" 10 | #include 11 | 12 | std::ostream& operator<< (std::ostream& stream, const DiscreteTheta& angle); 13 | 14 | namespace ugv_nav4d 15 | { 16 | 17 | class StateCreationFailed : public std::runtime_error {using std::runtime_error::runtime_error;}; 18 | class NodeCreationFailed : public std::runtime_error {using std::runtime_error::runtime_error;}; 19 | class ObstacleCheckFailed : public std::runtime_error {using std::runtime_error::runtime_error;}; 20 | class OrientationNotAllowed : public std::runtime_error {using std::runtime_error::runtime_error;}; 21 | 22 | 23 | class EnvironmentXYZTheta : public DiscreteSpaceInformation 24 | { 25 | protected: 26 | 27 | struct EnvironmentXYZThetaException : public SBPL_Exception 28 | { 29 | EnvironmentXYZThetaException(const std::string& what) : 30 | msg("SBPL has encountered a fatal error: " + what){} 31 | 32 | virtual const char* what() const throw() 33 | { 34 | return msg.c_str(); 35 | } 36 | const std::string msg; 37 | }; 38 | 39 | 40 | /** A discretized orientation */ 41 | struct ThetaNode 42 | { 43 | ThetaNode(const DiscreteTheta &t) :theta(t) {}; 44 | int id; 45 | DiscreteTheta theta; 46 | }; 47 | 48 | /**PlannerData is the userdata inside the XYZNode. */ 49 | struct PlannerData 50 | { 51 | PlannerData() : travNode(nullptr) {}; 52 | 53 | /**This is the node that was used to create this XYZNode.*/ 54 | traversability_generator3d::TravGenNode *travNode; 55 | 56 | /**An XYZNode is associated with every ThetaNode that it 57 | * shares a state with. This map links to all of them, 58 | * sorted by theta. */ 59 | std::map thetaToNodes; 60 | }; 61 | 62 | /** The distance from somewhere to start-node and goal-node.*/ 63 | struct Distance 64 | { 65 | double distToStart = 0; 66 | double distToGoal = 0; 67 | Distance(double toStart, double toGoal) : distToStart(toStart), distToGoal(toGoal){} 68 | }; 69 | 70 | /** A position on the traversability map */ 71 | typedef maps::grid::TraversabilityNode XYZNode; 72 | 73 | //search space without theta 74 | maps::grid::TraversabilityMap3d searchGrid; 75 | 76 | /** Represents one state in the search space */ 77 | struct Hash 78 | { 79 | Hash(XYZNode *node, ThetaNode *thetaNode) : node(node), thetaNode(thetaNode) 80 | { 81 | } 82 | XYZNode *node; /**< xyz position of the state and meta data */ 83 | ThetaNode *thetaNode;/** < angle of the state and additional meta data */ 84 | }; 85 | 86 | /**maps sbpl state ids to internal planner state (Hash). */ 87 | std::vector idToHash; 88 | 89 | /**Contains the distance from each travNode to start-node and goal-node 90 | * Stored in real-world coordinates (i.e. do NOT scale with gridResolution before use)*/ 91 | std::vector travNodeIdToDistance; 92 | std::shared_ptr travMap; 93 | 94 | PreComputedMotions availableMotions; 95 | 96 | ThetaNode *startThetaNode; 97 | XYZNode *startXYZNode; //part of the start state 98 | ThetaNode *goalThetaNode; 99 | XYZNode *goalXYZNode; //part of the goal state 100 | 101 | ThetaNode *createNewState(const DiscreteTheta& curTheta, EnvironmentXYZTheta::XYZNode* curNode); 102 | XYZNode *createNewXYZState(traversability_generator3d::TravGenNode* travNode); 103 | ThetaNode *createNewStateFromPose(const std::string& name, const Eigen::Vector3d& pos, double theta, ugv_nav4d::EnvironmentXYZTheta::XYZNode** xyzBackNode); 104 | 105 | bool checkStartGoalNode(const std::string& name, traversability_generator3d::TravGenNode* node, double theta); 106 | 107 | public: 108 | 109 | /** @param pos Position in map frame */ 110 | bool obstacleCheck(const maps::grid::Vector3d& pos, double theta, 111 | const traversability_generator3d::TraversabilityConfig& travConf, 112 | const sbpl_spline_primitives::SplinePrimitivesConfig& splineConf, 113 | const std::string& nodeName="node"); 114 | Eigen::Vector3d robotHalfSize; 115 | 116 | /** @param generateDebugData If true, lots of debug information will becollected 117 | * and stored in members starting with debug*/ 118 | EnvironmentXYZTheta(std::shared_ptr travMap, 119 | const traversability_generator3d::TraversabilityConfig &travConf, 120 | const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig, 121 | const Mobility& mobilityConfig); 122 | 123 | virtual ~EnvironmentXYZTheta(); 124 | 125 | void updateMap(std::shared_ptr travMap); 126 | 127 | virtual bool InitializeEnv(const char* sEnvFile); 128 | virtual bool InitializeMDPCfg(MDPConfig* MDPCfg); 129 | 130 | /**Returns the trajectory of least resistance to leave the obstacle. 131 | * @param start start position that is inside an obstacle 132 | * @param theta robot orientation 133 | * @param[out] outNewStart The new start position of the robot after it has moved out of the obstacle in the map frame 134 | * @return the best trajectory that gets the robot out of the obstacle. 135 | * Or an empty trajectory if no way out can be found*/ 136 | std::shared_ptr findTrajectoryOutOfObstacle(const Eigen::Vector3d& start, double theta, 137 | const Eigen::Affine3d& ground2Body, bool setZToZero); 138 | 139 | 140 | /** 141 | * \brief heuristic estimate from state FromStateID to state ToStateID 142 | */ 143 | virtual int GetFromToHeuristic(int FromStateID, int ToStateID); 144 | /** 145 | * \brief heuristic estimate from start state to state with stateID 146 | */ 147 | virtual int GetStartHeuristic(int stateID); 148 | /** 149 | * 150 | * \brief heuristic estimate from state with stateID to goal state 151 | */ 152 | virtual int GetGoalHeuristic(int stateID); 153 | 154 | virtual void GetPreds(int TargetStateID, std::vector< int >* PredIDV, std::vector< int >* CostV); 155 | virtual void GetSuccs(int SourceStateID, std::vector< int >* SuccIDV, std::vector< int >* CostV); 156 | virtual void GetSuccs(int SourceStateID, std::vector< int >* SuccIDV, std::vector< int >* CostV, std::vector< size_t >& motionIdV); 157 | 158 | virtual void PrintEnv_Config(FILE* fOut); 159 | virtual void PrintState(int stateID, bool bVerbose, FILE* fOut = 0); 160 | 161 | virtual void SetAllActionsandAllOutcomes(CMDPSTATE* state); 162 | virtual void SetAllPreds(CMDPSTATE* state); 163 | virtual int SizeofCreatedEnv(); 164 | 165 | void setStart(const Eigen::Vector3d &startPos, double theta); 166 | void setGoal(const Eigen::Vector3d &goalPos, double theta); 167 | 168 | maps::grid::Vector3d getStatePosition(const int stateID) const; 169 | 170 | traversability_generator3d::TravGenNode* findMatchingTraversabilityPatchAt(const Eigen::Vector3d &pos); 171 | 172 | /**returns the motion connection @p fromStateID and @p toStateID. 173 | * @throw std::runtime_error if no matching motion exists*/ 174 | const Motion& getMotion(const int fromStateID, const int toStateID); 175 | 176 | const std::shared_ptr getTraversabilityMap() const; 177 | 178 | std::vector getMotions(const std::vector &stateIDPath); 179 | 180 | void getTrajectory(const std::vector &stateIDPath, std::vector &result, 181 | bool setZToZero, const Eigen::Vector3d &startPos, const Eigen::Vector3d &goalPos, const double& goalHeading, const Eigen::Affine3d &plan2Body = Eigen::Affine3d::Identity()); 182 | 183 | const PreComputedMotions& getAvailableMotions() const; 184 | 185 | /**Clears the state of the environment. */ 186 | void clear(); 187 | 188 | void setTravConfig(const traversability_generator3d::TraversabilityConfig& cfg); 189 | 190 | /** @param maxDist The value that should be used as maximum distance. This value is used for 191 | * non-traversable nodes and for initialization.*/ 192 | void dijkstraComputeCost(const traversability_generator3d::TravGenNode* source, std::vector &outDistances, 193 | const double maxDist) const; 194 | 195 | /** Should a computationally expensive obstacle check be done to check whether the robot bounding box 196 | * is in collision with obstacles. This mode is useful for highly cluttered and tight spaced environments */ 197 | void enablePathStatistics(bool enable); 198 | 199 | private: 200 | 201 | /** Check if all nodes on the path from @p sourceNode following @p motion are traversable. 202 | * @return the target node of the motion or nullptr if motion not possible */ 203 | traversability_generator3d::TravGenNode* checkTraversableHeuristic(const maps::grid::Index sourceIndex, traversability_generator3d::TravGenNode* sourceNode, 204 | const ugv_nav4d::Motion& motion, const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode* >& trMap); 205 | 206 | /** Some movement directions are not allowed depending on the slope of the patch. 207 | * @return true if the movement direction is allowed on that patch 208 | */ 209 | bool checkOrientationAllowed(const traversability_generator3d::TravGenNode* node, 210 | const base::Orientation2D& orientation) const; 211 | 212 | 213 | /** Computes the heuristic */ 214 | void precomputeCost(); 215 | 216 | /**Return the avg slope of all patches on the given @p path */ 217 | double getAvgSlope(std::vector path) const; 218 | 219 | /**Returns the max slope of all patches on the given @p path */ 220 | double getMaxSlope(std::vector path) const; 221 | 222 | 223 | /**Determines the distance between @p a and @p b depending on travConf.heuristicType */ 224 | double getHeuristicDistance(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const; 225 | 226 | /** Checks if movement from @p fromTravNode to its neighbor at position @p toIdx is possible. 227 | * I.e. if a direct connection exists and if the neighbor is traversable. 228 | * Expands the neighbor if not already expanded. 229 | * @return the neighbor at position @p toIdx */ 230 | traversability_generator3d::TravGenNode* movementPossible(traversability_generator3d::TravGenNode* fromTravNode, const maps::grid::Index& fromIdx, const maps::grid::Index& toIdx); 231 | 232 | /** Expands @p node if it needs expansion. 233 | * Thread-safe. 234 | * @return True if the expansion succeeded */ 235 | bool checkExpandTreadSafe(traversability_generator3d::TravGenNode * node); 236 | 237 | bool usePathStatistics; 238 | 239 | traversability_generator3d::TraversabilityConfig travConf; 240 | sbpl_spline_primitives::SplinePrimitivesConfig primitiveConfig; 241 | 242 | unsigned int numAngles; 243 | 244 | Mobility mobilityConfig; 245 | }; 246 | 247 | } 248 | -------------------------------------------------------------------------------- /src/test/test_Planner.cpp: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_MODULE PlannerTestModule 2 | #include 3 | 4 | #include "ugv_nav4d/Planner.hpp" 5 | #include 6 | 7 | using namespace ugv_nav4d; 8 | 9 | class PlannerTest { 10 | public: 11 | PlannerTest(); 12 | ~PlannerTest(); 13 | std::string getResult(const Planner::PLANNING_RESULT& result); 14 | 15 | Planner* planner; 16 | maps::grid::MLSMapSloped* mlsMap; 17 | PlannerConfig plannerConfig; 18 | Mobility mobility; 19 | sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 20 | traversability_generator3d::TraversabilityConfig traversabilityConfig; 21 | }; 22 | 23 | PlannerTest::PlannerTest() { 24 | 25 | splinePrimitiveConfig.gridSize = 0.3; 26 | splinePrimitiveConfig.numAngles = 16; 27 | splinePrimitiveConfig.numEndAngles = 8; 28 | splinePrimitiveConfig.destinationCircleRadius = 6; 29 | splinePrimitiveConfig.cellSkipFactor = 0.1; 30 | splinePrimitiveConfig.splineOrder = 4.0; 31 | 32 | mobility.translationSpeed = 0.5; 33 | mobility.rotationSpeed = 0.5; 34 | mobility.minTurningRadius = 1; 35 | mobility.spline_sampling_resolution = 0.05; 36 | mobility.remove_goal_offset = true; 37 | mobility.multiplierForward = 1; 38 | mobility.multiplierBackward = 2; 39 | mobility.multiplierPointTurn = 3; 40 | mobility.multiplierLateral = 4; 41 | mobility.multiplierForwardTurn = 2; 42 | mobility.multiplierBackwardTurn = 3; 43 | mobility.multiplierLateralCurve = 4; 44 | mobility.searchRadius = 1.0; 45 | mobility.searchProgressSteps = 0.1; 46 | mobility.maxMotionCurveLength = 100; 47 | 48 | traversabilityConfig.maxStepHeight = 0.2; 49 | traversabilityConfig.maxSlope = 0.45; 50 | traversabilityConfig.inclineLimittingMinSlope = 0.2; 51 | traversabilityConfig.inclineLimittingLimit = 0.1; 52 | traversabilityConfig.costFunctionDist = 0.0; 53 | traversabilityConfig.minTraversablePercentage = 0.4; 54 | traversabilityConfig.robotHeight = 0.5; 55 | traversabilityConfig.robotSizeX = 0.5; 56 | traversabilityConfig.robotSizeY = 0.5; 57 | traversabilityConfig.distToGround = 0.0; 58 | traversabilityConfig.slopeMetricScale = 1.0; 59 | traversabilityConfig.slopeMetric = traversability_generator3d::NONE; 60 | traversabilityConfig.gridResolution = 0.3; 61 | traversabilityConfig.initialPatchVariance = 0.0001; 62 | traversabilityConfig.allowForwardDownhill = true; 63 | traversabilityConfig.enableInclineLimitting = false; 64 | 65 | plannerConfig.searchUntilFirstSolution = false; 66 | plannerConfig.initialEpsilon = 64; 67 | plannerConfig.epsilonSteps = 2; 68 | plannerConfig.numThreads = 4; 69 | 70 | maps::grid::Vector2d res(0.3, 0.3); 71 | maps::grid::Vector2ui numCells(100, 100); 72 | 73 | maps::grid::MLSConfig mls_config; 74 | mls_config.gapSize = 0.1; 75 | mls_config.updateModel = maps::grid::MLSConfig::SLOPE; 76 | 77 | mlsMap = new maps::grid::MLSMapSloped(numCells, res, mls_config); 78 | 79 | /** Translate the local frame (offset) **/ 80 | mlsMap->getLocalFrame().translation() << 0.5*mlsMap->getSize(), 0; 81 | 82 | Eigen::Vector2d max = 0.5 * mlsMap->getSize(); 83 | Eigen::Vector2d min = -0.5 * mlsMap->getSize(); 84 | 85 | for (double x = min.x(); x < max.x(); x += 0.00625) 86 | { 87 | for (double y = min.y(); y < max.y(); y += 0.00625) 88 | { 89 | mlsMap->mergePoint(Eigen::Vector3d(x, y, 0)); 90 | } 91 | } 92 | } 93 | 94 | PlannerTest::~PlannerTest() { 95 | delete planner; 96 | delete mlsMap; 97 | } 98 | 99 | std::string PlannerTest::getResult(const Planner::PLANNING_RESULT& result) { 100 | switch (result) { 101 | case Planner::GOAL_INVALID: return "GOAL_INVALID"; 102 | case Planner::START_INVALID: return "START_INVALID"; 103 | case Planner::NO_SOLUTION: return "NO_SOLUTION"; 104 | case Planner::NO_MAP: return "NO_MAP"; 105 | case Planner::INTERNAL_ERROR: return "INTERNAL_ERROR"; 106 | case Planner::FOUND_SOLUTION: return "FOUND_SOLUTION"; 107 | default: return "ERROR unknown result state"; 108 | } 109 | } 110 | 111 | BOOST_FIXTURE_TEST_SUITE(PlannerTestSuite, PlannerTest) 112 | 113 | BOOST_AUTO_TEST_CASE(check_planner_init_success) { 114 | planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); 115 | BOOST_CHECK(planner != nullptr); 116 | } 117 | 118 | BOOST_AUTO_TEST_CASE(check_planner_goal_invalid) { 119 | planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); 120 | BOOST_REQUIRE(planner != nullptr); 121 | planner->updateMap(*mlsMap); 122 | 123 | base::samples::RigidBodyState startState; 124 | startState.position.x() = 0; 125 | startState.position.y() = 0; 126 | startState.position.z() = 0; 127 | startState.orientation = Eigen::Quaterniond(1,0,0,0); 128 | 129 | base::samples::RigidBodyState endState; 130 | endState.position.x() = 40.0; 131 | endState.position.y() = 0.0; 132 | endState.position.z() = 0.0; 133 | endState.orientation = Eigen::Quaterniond(1,0,0,0); 134 | 135 | int maxTime = 5; 136 | std::vector trajectory2D; 137 | std::vector trajectory3D; 138 | 139 | const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); 140 | BOOST_CHECK_EQUAL(result, Planner::GOAL_INVALID); 141 | } 142 | 143 | BOOST_AUTO_TEST_CASE(check_planner_start_invalid) { 144 | planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); 145 | BOOST_REQUIRE(planner != nullptr); 146 | planner->updateMap(*mlsMap); 147 | 148 | base::samples::RigidBodyState startState; 149 | startState.position.x() = 40.0; 150 | startState.position.y() = 0.0; 151 | startState.position.z() = 0.0; 152 | startState.orientation = Eigen::Quaterniond(1,0,0,0); 153 | 154 | base::samples::RigidBodyState endState; 155 | endState.position.x() = 1.0; 156 | endState.position.y() = 0.0; 157 | endState.position.z() = 0.0; 158 | endState.orientation = Eigen::Quaterniond(1,0,0,0); 159 | 160 | int maxTime = 5; 161 | std::vector trajectory2D; 162 | std::vector trajectory3D; 163 | 164 | const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); 165 | BOOST_CHECK_EQUAL(result, Planner::START_INVALID); 166 | } 167 | 168 | BOOST_AUTO_TEST_CASE(check_planner_success) { 169 | planner = new Planner(splinePrimitiveConfig, traversabilityConfig, mobility, plannerConfig); 170 | BOOST_REQUIRE(planner != nullptr); 171 | planner->updateMap(*mlsMap); 172 | 173 | base::samples::RigidBodyState startState; 174 | startState.position.x() = 2.3; 175 | startState.position.y() = 4.1; 176 | startState.position.z() = 0.0; 177 | startState.orientation = Eigen::Quaterniond(1,0,0,0); 178 | 179 | base::samples::RigidBodyState endState; 180 | endState.position.x() = 6.1; 181 | endState.position.y() = 4.2; 182 | endState.position.z() = 0.0; 183 | endState.orientation = Eigen::Quaterniond(1,0,0,0); 184 | 185 | int maxTime = 5; 186 | std::vector trajectory2D; 187 | std::vector trajectory3D; 188 | 189 | const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D); 190 | BOOST_CHECK_EQUAL(result, Planner::FOUND_SOLUTION); 191 | BOOST_CHECK_GT(trajectory2D.size(), 0); 192 | BOOST_CHECK_GT(trajectory3D.size(), 0); 193 | BOOST_CHECK_EQUAL(trajectory2D.size(), trajectory3D.size()); 194 | 195 | //Start and Goal Positions 196 | BOOST_REQUIRE(trajectory2D.front().startPose.position.isApprox(startState.position.head<2>(), 1e-6)); 197 | BOOST_REQUIRE(trajectory2D.back().goalPose.position.isApprox(endState.position.head<2>(), 1e-6)); 198 | BOOST_REQUIRE(trajectory2D.front().posSpline.getStartPoint().isApprox(startState.position, 1e-6)); 199 | BOOST_REQUIRE(trajectory2D.back().posSpline.getEndPoint().isApprox(endState.position, 1e-6)); 200 | 201 | BOOST_REQUIRE(trajectory3D.front().startPose.position.isApprox(startState.position.head<2>(), 1e-6)); 202 | BOOST_REQUIRE(trajectory3D.back().goalPose.position.isApprox(endState.position.head<2>(), 1e-6)); 203 | BOOST_REQUIRE(trajectory3D.front().posSpline.getStartPoint().isApprox(startState.position, 1e-6)); 204 | BOOST_REQUIRE(trajectory3D.back().posSpline.getEndPoint().isApprox(endState.position, 1e-6)); 205 | 206 | 207 | //Start and Goal Orientations 208 | BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory2D.front().startPose.orientation).getRad(), 209 | base::Angle::fromRad(base::getYaw(startState.orientation)).getRad()); 210 | BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory2D.back().goalPose.orientation).getRad(), 211 | base::Angle::fromRad(base::getYaw(endState.orientation)).getRad()); 212 | 213 | BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory3D.front().startPose.orientation).getRad(), 214 | base::Angle::fromRad(base::getYaw(startState.orientation)).getRad()); 215 | BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory3D.back().goalPose.orientation).getRad(), 216 | base::Angle::fromRad(base::getYaw(endState.orientation)).getRad()); 217 | 218 | for (auto& trajectory : trajectory2D){ 219 | BOOST_REQUIRE(trajectory.speed <= mobility.translationSpeed); 220 | BOOST_REQUIRE(trajectory.speed <= mobility.rotationSpeed); 221 | 222 | if (trajectory.driveMode != trajectory_follower::ModeTurnOnTheSpot){ 223 | BOOST_REQUIRE(trajectory.posSpline.isEmpty() == false); 224 | 225 | BOOST_REQUIRE(trajectory.getCurvatureMax() <= mobility.minTurningRadius/2); 226 | BOOST_REQUIRE(trajectory.posSpline.getCurveLength() <= mobility.maxMotionCurveLength); 227 | } 228 | else{ 229 | BOOST_REQUIRE(trajectory.orientationSpline.isEmpty() == false); 230 | 231 | BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getStartParam()).orientation).getRad(), 232 | base::Angle::fromRad(trajectory.startPose.orientation).getRad(), 1e-6); 233 | 234 | BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getEndParam()).orientation).getRad(), 235 | base::Angle::fromRad(trajectory.goalPose.orientation).getRad(), 1e-6); 236 | } 237 | } 238 | 239 | for (auto& trajectory : trajectory3D){ 240 | BOOST_REQUIRE(trajectory.speed <= mobility.translationSpeed); 241 | BOOST_REQUIRE(trajectory.speed <= mobility.rotationSpeed); 242 | 243 | if (trajectory.driveMode != trajectory_follower::ModeTurnOnTheSpot){ 244 | BOOST_REQUIRE(trajectory.posSpline.isEmpty() == false); 245 | 246 | BOOST_REQUIRE(trajectory.getCurvatureMax() <= mobility.minTurningRadius/2); 247 | BOOST_REQUIRE(trajectory.posSpline.getCurveLength() <= mobility.maxMotionCurveLength); 248 | } 249 | else{ 250 | BOOST_REQUIRE(trajectory.orientationSpline.isEmpty() == false); 251 | 252 | BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getStartParam()).orientation).getRad(), 253 | base::Angle::fromRad(trajectory.startPose.orientation).getRad(), 1e-6); 254 | 255 | BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getEndParam()).orientation).getRad(), 256 | base::Angle::fromRad(trajectory.goalPose.orientation).getRad(), 1e-6); 257 | } 258 | } 259 | 260 | 261 | } 262 | 263 | 264 | BOOST_AUTO_TEST_SUITE_END() -------------------------------------------------------------------------------- /src/PathStatistic.cpp: -------------------------------------------------------------------------------- 1 | #include "PathStatistic.hpp" 2 | #include 3 | #include 4 | 5 | #ifdef ENABLE_DEBUG_DRAWINGS 6 | #include 7 | #include 8 | #endif 9 | 10 | ugv_nav4d::PathStatistic::Stats::Stats() : 11 | obstacles(0), 12 | frontiers(0) 13 | { 14 | //check that all patch types (that we use in here) are valid array indices 15 | //otherwise we might access the array out of bounds 16 | static_assert(maps::grid::TraversabilityNodeBase::OBSTACLE < maps::grid::TraversabilityNodeBase::FRONTIER + 1, ""); 17 | static_assert(maps::grid::TraversabilityNodeBase::TRAVERSABLE < maps::grid::TraversabilityNodeBase::FRONTIER + 1, ""); 18 | static_assert(maps::grid::TraversabilityNodeBase::UNKNOWN < maps::grid::TraversabilityNodeBase::FRONTIER + 1, ""); 19 | static_assert(maps::grid::TraversabilityNodeBase::HOLE < maps::grid::TraversabilityNodeBase::FRONTIER + 1, ""); 20 | static_assert(maps::grid::TraversabilityNodeBase::UNSET < maps::grid::TraversabilityNodeBase::FRONTIER + 1, ""); 21 | static_assert(maps::grid::TraversabilityNodeBase::FRONTIER < maps::grid::TraversabilityNodeBase::FRONTIER + 1, ""); 22 | 23 | 24 | minDistance.resize(maps::grid::TraversabilityNodeBase::FRONTIER + 1, std::numeric_limits< double >::max()); 25 | 26 | 27 | minDistToObstacle = std::numeric_limits< double >::max(); 28 | } 29 | 30 | void ugv_nav4d::PathStatistic::Stats::updateStatistic(const maps::grid::TraversabilityNodeBase* node) 31 | { 32 | switch(node->getType()) 33 | { 34 | case maps::grid::TraversabilityNodeBase::TRAVERSABLE: 35 | break; 36 | case maps::grid::TraversabilityNodeBase::FRONTIER: 37 | frontiers++; 38 | break; 39 | default: 40 | minDistToObstacle = std::min(minDistance[node->getType()], minDistToObstacle); 41 | obstacles++; 42 | break; 43 | } 44 | } 45 | 46 | void ugv_nav4d::PathStatistic::Stats::updateDistance(const maps::grid::TraversabilityNodeBase* node, double distance) 47 | { 48 | minDistance[node->getType()] = std::min(minDistance[node->getType()], distance); 49 | } 50 | 51 | double ugv_nav4d::PathStatistic::Stats::getMinDistToFrontiers() const 52 | { 53 | return minDistance[maps::grid::TraversabilityNodeBase::FRONTIER]; 54 | } 55 | 56 | double ugv_nav4d::PathStatistic::Stats::getMinDistToObstacles() const 57 | { 58 | return minDistToObstacle; 59 | } 60 | 61 | ugv_nav4d::PathStatistic::PathStatistic(const traversability_generator3d::TraversabilityConfig& config) : 62 | config(config) 63 | { 64 | } 65 | 66 | void ugv_nav4d::PathStatistic::calculateStatistics(const std::vector& path, 67 | const std::vector< base::Pose2D >& poses, 68 | const traversability_generator3d::TravMap3d& trMap, 69 | const std::string &debugObstacleName) 70 | { 71 | assert(path.size() == poses.size()); 72 | // CLEAR_DRAWING("CollisionBox"); 73 | 74 | const Eigen::Vector2d travGridResolution(config.gridResolution, config.gridResolution); 75 | 76 | Eigen::Vector2d halfRobotDimension(config.robotSizeX / 2.0, config.robotSizeY/2.0); 77 | Eigen::Vector2d halfOuterBoxDimension(halfRobotDimension + Eigen::Vector2d::Constant(config.costFunctionDist)); 78 | 79 | std::vector edgePositions = { 80 | Eigen::Vector2d(- config.gridResolution /2.0, - config.gridResolution / 2.0), 81 | Eigen::Vector2d(- config.gridResolution / 2.0, config.gridResolution / 2.0), 82 | Eigen::Vector2d(config.gridResolution / 2.0, config.gridResolution / 2.0), 83 | Eigen::Vector2d(config.gridResolution / 2.0, -config.gridResolution / 2.0) 84 | }; 85 | 86 | Eigen::AlignedBox robotBoundingBox(- halfRobotDimension, halfRobotDimension); 87 | Eigen::AlignedBox costFunctionBoundingBox(- halfOuterBoxDimension, halfOuterBoxDimension); 88 | 89 | std::unordered_set inRobot; 90 | std::unordered_set inBoundary; 91 | 92 | for(size_t i = 0; i < path.size(); i++) 93 | { 94 | const traversability_generator3d::TravGenNode *node(path[i]); 95 | const base::Pose2D curPose(poses[i]); 96 | 97 | const Eigen::Rotation2D yawInverse(Eigen::Rotation2D(curPose.orientation).inverse()); 98 | 99 | maps::grid::Vector3d nodePos3 = node->getPosition(trMap); 100 | const maps::grid::Vector2d nodePos(nodePos3.head<2>()); 101 | 102 | bool hasObstacle = false; 103 | 104 | node->eachConnectedNode([&] (const maps::grid::TraversabilityNodeBase *neighbor, bool &explandNode, bool &stop){ 105 | //we need to compute the four edges of a cell and check if any is inside of the robot 106 | maps::grid::Vector3d neighborPos; 107 | trMap.fromGrid(neighbor->getIndex(), neighborPos, neighbor->getHeight(), false); 108 | 109 | bool isInsideRobot = false; 110 | bool isInsideOuterBox = false; 111 | 112 | for(const Eigen::Vector2d &ep : edgePositions) 113 | { 114 | const Eigen::Vector2d edgePos(neighborPos.head<2>() + ep); 115 | 116 | //translate to center 117 | Eigen::Vector2d tp = edgePos - curPose.position; 118 | //rotate invers to orientation of robot 119 | tp = yawInverse * tp; 120 | 121 | if(costFunctionBoundingBox.contains(tp)) 122 | { 123 | isInsideOuterBox = true; 124 | 125 | //it is only inside the robot 126 | if(robotBoundingBox.contains(tp)) 127 | { 128 | isInsideRobot = true; 129 | robotStats.updateDistance(neighbor, (nodePos3 - neighborPos).head<2>().norm()); 130 | //we continue iteration here, to compute the correct distances of all edges 131 | } 132 | else 133 | { 134 | boundaryStats.updateDistance(neighbor, robotBoundingBox.exteriorDistance(tp)); 135 | } 136 | } 137 | } 138 | 139 | if(!isInsideOuterBox) 140 | { 141 | return; 142 | } 143 | 144 | //further expand node 145 | explandNode = true; 146 | 147 | if(isInsideRobot) 148 | { 149 | inRobot.insert(neighbor); 150 | 151 | if(neighbor->getType() != maps::grid::TraversabilityNodeBase::TRAVERSABLE) 152 | { 153 | #ifdef ENABLE_DEBUG_DRAWINGS 154 | V3DD::COMPLEX_DRAWING([&]() 155 | { 156 | if(!debugObstacleName.empty()) 157 | { 158 | V3DD::DRAW_ARROW(debugObstacleName, 159 | neighborPos, 160 | Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())), Eigen::Vector3d(.3, 0.3, 0.8), V3DD::Color::red); 161 | } 162 | }); 163 | #endif 164 | hasObstacle = true; 165 | stop = true; 166 | } 167 | 168 | return; 169 | } 170 | 171 | inBoundary.insert(neighbor); 172 | 173 | }); 174 | /* 175 | if(hasObstacle) 176 | { 177 | COMPLEX_DRAWING ( 178 | 179 | Eigen::Vector3d checkPos; 180 | checkPos.head<2>() = curPose.position; 181 | checkPos.z() = node->getHeight(); 182 | 183 | DRAW_WIREFRAME_BOX("CollisionBox", checkPos, Eigen::Quaterniond(Eigen::AngleAxisd(curPose.orientation, Eigen::Vector3d::UnitZ())), Eigen::Vector3d(config.robotSizeX, config.robotSizeY, config.robotHeight), vizkit3dDebugDrawings::Color::red); 184 | ); 185 | }*/ 186 | 187 | if(hasObstacle) 188 | break; 189 | } 190 | 191 | for(const maps::grid::TraversabilityNodeBase* n : inBoundary) 192 | { 193 | if(inRobot.find(n) == inRobot.end()) 194 | { 195 | boundaryStats.updateStatistic(n); 196 | } 197 | } 198 | 199 | for(const maps::grid::TraversabilityNodeBase* n : inRobot) 200 | { 201 | robotStats.updateStatistic(n); 202 | } 203 | 204 | } 205 | 206 | bool ugv_nav4d::PathStatistic::isPathFeasible(const std::vector& path, 207 | const std::vector< base::Pose2D >& poses, 208 | const traversability_generator3d::TravMap3d& trMap) 209 | { 210 | assert(path.size() == poses.size()); 211 | 212 | const Eigen::Vector2d travGridResolution(config.gridResolution, config.gridResolution); 213 | 214 | Eigen::Vector2d halfRobotDimension(config.robotSizeX / 2.0, config.robotSizeY/2.0); 215 | Eigen::Vector2d halfOuterBoxDimension(halfRobotDimension + Eigen::Vector2d::Constant(config.costFunctionDist)); 216 | 217 | std::vector edgePositions = { 218 | Eigen::Vector2d(- config.gridResolution /2.0, - config.gridResolution / 2.0), 219 | Eigen::Vector2d(- config.gridResolution / 2.0, config.gridResolution / 2.0), 220 | Eigen::Vector2d(config.gridResolution / 2.0, config.gridResolution / 2.0), 221 | Eigen::Vector2d(config.gridResolution / 2.0, -config.gridResolution / 2.0) 222 | }; 223 | 224 | Eigen::AlignedBox robotBoundingBox(- halfRobotDimension, halfRobotDimension); 225 | Eigen::AlignedBox costFunctionBoundingBox(- halfOuterBoxDimension, halfOuterBoxDimension); 226 | 227 | std::unordered_set inRobot; 228 | std::unordered_set inBoundary; 229 | 230 | for(size_t i = 0; i < path.size(); i++) 231 | { 232 | const traversability_generator3d::TravGenNode *node(path[i]); 233 | const base::Pose2D curPose(poses[i]); 234 | 235 | const Eigen::Rotation2D yawInverse(Eigen::Rotation2D(curPose.orientation).inverse()); 236 | 237 | maps::grid::Vector3d nodePos3 = node->getPosition(trMap); 238 | const maps::grid::Vector2d nodePos(nodePos3.head<2>()); 239 | 240 | bool hasObstacle = false; 241 | 242 | node->eachConnectedNode([&] (const maps::grid::TraversabilityNodeBase *neighbor, bool &explandNode, bool &stop){ 243 | //we need to compute the four edges of a cell and check if any is inside of the robot 244 | maps::grid::Vector3d neighborPos; 245 | trMap.fromGrid(neighbor->getIndex(), neighborPos, neighbor->getHeight(), false); 246 | 247 | bool isInsideRobot = false; 248 | bool isInsideOuterBox = false; 249 | 250 | for(const Eigen::Vector2d &ep : edgePositions) 251 | { 252 | const Eigen::Vector2d edgePos(neighborPos.head<2>() + ep); 253 | 254 | //translate to center 255 | Eigen::Vector2d tp = edgePos - curPose.position; 256 | //rotate invers to orientation of robot 257 | tp = yawInverse * tp; 258 | 259 | if(costFunctionBoundingBox.contains(tp)) 260 | { 261 | isInsideOuterBox = true; 262 | 263 | //it is only inside the robot 264 | if(robotBoundingBox.contains(tp)) 265 | { 266 | isInsideRobot = true; 267 | break; 268 | } 269 | } 270 | } 271 | 272 | if(!isInsideOuterBox) 273 | { 274 | return; 275 | } 276 | 277 | //further expand node 278 | explandNode = true; 279 | 280 | if(isInsideRobot) 281 | { 282 | if(neighbor->getType() != maps::grid::TraversabilityNodeBase::TRAVERSABLE) 283 | { 284 | hasObstacle = true; 285 | stop = true; 286 | } 287 | 288 | return; 289 | } 290 | 291 | }); 292 | if(hasObstacle) 293 | return false; 294 | } 295 | return true; 296 | } 297 | -------------------------------------------------------------------------------- /source_dependencies/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | include(ExternalProject) 3 | 4 | ExternalProject_Add(base-cmake 5 | PREFIX base/cmake 6 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/cmake/stamp 7 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/cmake/src 8 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/cmake/src 9 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/cmake/build 10 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 11 | GIT_REPOSITORY https://github.com/rock-core/base-cmake.git 12 | GIT_TAG master 13 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 14 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 15 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 16 | ) 17 | 18 | ExternalProject_Add(planning-sbpl 19 | PREFIX planning/sbpl 20 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl/stamp 21 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl/src 22 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl/src 23 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl/build 24 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 25 | GIT_REPOSITORY https://github.com/sbpl/sbpl.git 26 | GIT_TAG master 27 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 28 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 29 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 30 | ) 31 | 32 | ExternalProject_Add(planning-sbpl_spline_primitives 33 | PREFIX planning/sbpl_spline_primitives 34 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl_spline_primitives/stamp 35 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl_spline_primitives/src 36 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl_spline_primitives/src 37 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/sbpl_spline_primitives/build 38 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 39 | GIT_REPOSITORY https://github.com/pierrewillenbrockdfki/planning-sbpl_spline_primitives.git 40 | GIT_TAG feature/qt4-qt5 41 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 42 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 43 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 44 | ) 45 | add_dependencies(planning-sbpl_spline_primitives base-cmake base-types ) 46 | 47 | ExternalProject_Add(planning-traversability_generator3d 48 | PREFIX planning/traversability_generator3d 49 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/traversability_generator3d/stamp 50 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/traversability_generator3d/src 51 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/traversability_generator3d/src 52 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/planning/traversability_generator3d/build 53 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 54 | GIT_REPOSITORY https://github.com/dfki-ric/traversability_generator3d.git 55 | GIT_TAG main 56 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 57 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 58 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 59 | ) 60 | add_dependencies(planning-traversability_generator3d base-cmake slam-maps gui-vizkit3d_debug_drawings ) 61 | 62 | ExternalProject_Add(gui-vizkit3d_debug_drawings 63 | PREFIX gui/vizkit3d_debug_drawings 64 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d_debug_drawings/stamp 65 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d_debug_drawings/src 66 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d_debug_drawings/src 67 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d_debug_drawings/build 68 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 69 | GIT_REPOSITORY https://github.com/pierrewillenbrockdfki/gui-vizkit3d_debug_drawings.git 70 | GIT_TAG feature/qt4-qt5 71 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 72 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 73 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DWITH_PORTS=OFF -DCMAKE_BUILD_TYPE=RELEASE 74 | ) 75 | add_dependencies(gui-vizkit3d_debug_drawings base-cmake gui-vizkit3d gui-osgviz ) 76 | 77 | ExternalProject_Add(control-trajectory_follower 78 | PREFIX control/trajectory_follower 79 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/control/trajectory_follower/stamp 80 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/control/trajectory_follower/src 81 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/control/trajectory_follower/src 82 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/control/trajectory_follower/build 83 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 84 | GIT_REPOSITORY https://github.com/pierrewillenbrockdfki/control-trajectory_follower.git 85 | GIT_TAG limit_speed_at_end_of_traj-qt4-qt5 86 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 87 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 88 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 89 | ) 90 | add_dependencies(control-trajectory_follower base-types external-sisl gui-osgviz ) 91 | 92 | ExternalProject_Add(external-googletest 93 | PREFIX external/googletest 94 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/googletest/stamp 95 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/googletest/src 96 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/googletest/src 97 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/googletest/build 98 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 99 | GIT_REPOSITORY https://github.com/google/googletest.git 100 | GIT_TAG main 101 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 102 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 103 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 104 | ) 105 | 106 | ExternalProject_Add(base-types 107 | PREFIX base/types 108 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/types/stamp 109 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/types/src 110 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/types/src 111 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/types/build 112 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 113 | GIT_REPOSITORY https://github.com/pierrewillenbrockdfki/base-types.git 114 | GIT_TAG feature/qt4-qt5 115 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 116 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 117 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DUSE_SISL=ON -DBINDINGS_RUBY=OFF -DROCK_VIZ_ENABLED=TRUE -DCMAKE_BUILD_TYPE=RELEASE 118 | ) 119 | add_dependencies(base-types base-cmake base-logging external-sisl gui-vizkit3d ) 120 | 121 | ExternalProject_Add(slam-maps 122 | PREFIX slam/maps 123 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/slam/maps/stamp 124 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/slam/maps/src 125 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/slam/maps/src 126 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/slam/maps/build 127 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 128 | GIT_REPOSITORY https://github.com/haider8645/slam-maps.git 129 | GIT_TAG feature/qt4-qt5 130 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 131 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 132 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 133 | ) 134 | add_dependencies(slam-maps base-cmake base-types base-numeric base-boost_serialization gui-vizkit3d gui-osgviz ) 135 | 136 | ExternalProject_Add(gui-vizkit3d 137 | PREFIX gui/vizkit3d 138 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d/stamp 139 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d/src 140 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d/src 141 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/vizkit3d/build 142 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 143 | GIT_REPOSITORY https://github.com/pierrewillenbrockdfki/gui-vizkit3d.git 144 | GIT_TAG feature/qt4-qt5 145 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 146 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 147 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 148 | ) 149 | add_dependencies(gui-vizkit3d base-cmake gui-osgviz gui-osg_qt5 gui-qtpropertybrowser ) 150 | 151 | ExternalProject_Add(gui-osgviz 152 | PREFIX gui/osgviz 153 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osgviz/stamp 154 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osgviz/src 155 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osgviz/src 156 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osgviz/build 157 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 158 | GIT_REPOSITORY https://github.com/rock-core/gui-osgviz.git 159 | GIT_TAG master 160 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 161 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 162 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 163 | ) 164 | add_dependencies(gui-osgviz base-cmake ) 165 | 166 | ExternalProject_Add(external-sisl 167 | PREFIX external/sisl 168 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/sisl/stamp 169 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/sisl/src 170 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/sisl/src 171 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/external/sisl/build 172 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 173 | GIT_REPOSITORY https://github.com/SINTEF-Geometry/SISL.git 174 | GIT_TAG master 175 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 176 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 177 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 178 | ) 179 | 180 | ExternalProject_Add(base-logging 181 | PREFIX base/logging 182 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/logging/stamp 183 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/logging/src 184 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/logging/src 185 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/logging/build 186 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 187 | GIT_REPOSITORY https://github.com/rock-core/base-logging.git 188 | GIT_TAG master 189 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 190 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 191 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 192 | ) 193 | add_dependencies(base-logging base-cmake ) 194 | 195 | ExternalProject_Add(base-numeric 196 | PREFIX base/numeric 197 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/numeric/stamp 198 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/numeric/src 199 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/numeric/src 200 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/numeric/build 201 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 202 | GIT_REPOSITORY https://github.com/rock-core/base-numeric.git 203 | GIT_TAG master 204 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 205 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 206 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 207 | ) 208 | add_dependencies(base-numeric base-types base-logging ) 209 | 210 | ExternalProject_Add(base-boost_serialization 211 | PREFIX base/boost_serialization 212 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/boost_serialization/stamp 213 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/boost_serialization/src 214 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/boost_serialization/src 215 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/base/boost_serialization/build 216 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 217 | GIT_REPOSITORY https://github.com/envire/base-boost_serialization.git 218 | GIT_TAG master 219 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 220 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 221 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 222 | ) 223 | add_dependencies(base-boost_serialization base-cmake base-types base-numeric ) 224 | 225 | ExternalProject_Add(gui-osg_qt5 226 | PREFIX gui/osg_qt5 227 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osg_qt5/stamp 228 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osg_qt5/src 229 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osg_qt5/src 230 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/osg_qt5/build 231 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 232 | GIT_REPOSITORY https://github.com/rock-core/gui-osg_qt4.git 233 | GIT_TAG feature/qt4-qt5 234 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 235 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 236 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 237 | ) 238 | add_dependencies(gui-osg_qt5 base-cmake ) 239 | 240 | ExternalProject_Add(gui-qtpropertybrowser 241 | PREFIX gui/qtpropertybrowser 242 | STAMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/qtpropertybrowser/stamp 243 | DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/qtpropertybrowser/src 244 | SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/qtpropertybrowser/src 245 | BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/gui/qtpropertybrowser/build 246 | INSTALL_DIR ${CMAKE_INSTALL_PREFIX} 247 | GIT_REPOSITORY https://github.com/abhijitkundu/QtPropertyBrowser.git 248 | GIT_TAG master 249 | CMAKE_CACHE_ARGS -DENV{PKG_CONFIG_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ 250 | -DENV{CMAKE_PREFIX_PATH}:STRING=${CMAKE_INSTALL_PREFIX}/share/rock/cmake/ 251 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_BUILD_TYPE=RELEASE 252 | ) 253 | 254 | configure_file(env.sh.in ${PROJECT_SOURCE_DIR}/env.sh) 255 | install(FILES ${PROJECT_SOURCE_DIR}/env.sh DESTINATION ${CMAKE_INSTALL_PREFIX}) 256 | -------------------------------------------------------------------------------- /src/PreComputedMotions.cpp: -------------------------------------------------------------------------------- 1 | #include "PreComputedMotions.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | namespace ugv_nav4d 8 | { 9 | using namespace sbpl_spline_primitives; 10 | 11 | 12 | PreComputedMotions::PreComputedMotions(const SplinePrimitivesConfig& primitiveConfig, 13 | const Mobility& mobilityConfig): 14 | primitives(primitiveConfig), 15 | mobilityConfig(mobilityConfig) 16 | { 17 | } 18 | 19 | 20 | void PreComputedMotions::computeMotions(double travGridResolution) 21 | { 22 | if(fabs(primitives.getConfig().gridSize - travGridResolution) > 1E-5) 23 | { 24 | LOG_ERROR_S << "PreComputedMotions::computeMotions: Error grid size and trav size do not match"; 25 | throw std::runtime_error("PreComputedMotions::computeMotions: Error grid size and trav size do not match"); 26 | } 27 | 28 | readMotionPrimitives(primitives, mobilityConfig, travGridResolution); 29 | } 30 | 31 | void PreComputedMotions::sampleOnResolution(double gridResolution,base::geometry::Spline2 spline, std::vector &result, std::vector &fullResult) 32 | { 33 | maps::grid::GridMap dummyGrid(maps::grid::Vector2ui(10, 10), base::Vector2d(gridResolution, gridResolution), 0); 34 | 35 | CellWithPoses curPoses; 36 | curPoses.cell = maps::grid::Index(0,0); 37 | 38 | //sample spline: 39 | const double stepDist = mobilityConfig.spline_sampling_resolution; 40 | std::vector parameters; 41 | //NOTE we dont need the points, but there is no sample() api that returns parameters only 42 | const std::vector points = spline.sample(stepDist, ¶meters); 43 | assert(parameters.size() == points.size()); 44 | for(size_t i = 0; i < parameters.size(); ++i) 45 | { 46 | const double param = parameters[i]; 47 | base::Vector2d point, tangent; 48 | std::tie(point,tangent) = spline.getPointAndTangent(param); 49 | const base::Orientation2D orientation(std::atan2(tangent.y(), tangent.x())); 50 | 51 | 52 | const base::Pose2D pose(point, orientation); 53 | 54 | //point needs to be offset to the middle of the grid, 55 | //as all path computation also starts in the middle 56 | //if not we would get a wrong diff 57 | point += base::Vector2d(gridResolution /2.0, gridResolution /2.0); 58 | 59 | maps::grid::Index diff; 60 | if(!dummyGrid.toGrid(base::Vector3d(point.x(), point.y(), 0), diff, false)) 61 | throw std::runtime_error("Internal Error : Cannot convert intermediate Pose to grid cell"); 62 | 63 | if(curPoses.cell != diff) 64 | { 65 | fullResult.push_back(curPoses); 66 | 67 | //Find best match for collision check 68 | PoseWithCell pwc; 69 | pwc.cell = curPoses.cell; 70 | pwc.pose = getPointClosestToCellMiddle(curPoses, gridResolution); 71 | result.push_back(pwc); 72 | 73 | curPoses.poses.clear(); 74 | curPoses.cell = diff; 75 | } 76 | 77 | curPoses.poses.push_back(pose); 78 | } 79 | 80 | fullResult.push_back(curPoses); 81 | 82 | //Find best match for collision check 83 | PoseWithCell pwc; 84 | pwc.cell = curPoses.cell; 85 | pwc.pose = getPointClosestToCellMiddle(curPoses, gridResolution); 86 | result.push_back(pwc); 87 | 88 | assert(result.size() > 0); //at least the end pose should always be part of the steps 89 | 90 | } 91 | 92 | void PreComputedMotions::readMotionPrimitives(const SbplSplineMotionPrimitives& primGen, 93 | const Mobility& mobilityConfig, double travGridResolution) 94 | { 95 | const int numAngles = primGen.getConfig().numAngles; 96 | const double maxCurvature = calculateCurvatureFromRadius(mobilityConfig.minTurningRadius); 97 | 98 | for(int angle = 0; angle < numAngles; ++angle) 99 | { 100 | for(const SplinePrimitive& prim : primGen.getPrimitiveForAngle(angle)) 101 | { 102 | //NOTE the const cast is only here because for some reason getCurvatureMax() is non-const (but shouldnt be) 103 | if(prim.motionType != SplinePrimitive::SPLINE_POINT_TURN && //cannot call getCurvatureMax on point turns cause spl ne is not initalized 104 | const_cast(prim).spline.getCurvatureMax() > maxCurvature) 105 | { 106 | continue; 107 | } 108 | 109 | Motion motion(numAngles); 110 | 111 | motion.xDiff = prim.endPosition[0]; 112 | motion.yDiff = prim.endPosition[1]; 113 | motion.endTheta = DiscreteTheta(static_cast(prim.endAngle), numAngles); 114 | motion.startTheta = DiscreteTheta(static_cast(prim.startAngle), numAngles); 115 | motion.costMultiplier = 1; //is changed in the switch-case below 116 | 117 | switch(prim.motionType) 118 | { 119 | case SplinePrimitive::SPLINE_MOVE_FORWARD: 120 | motion.type = Motion::Type::MOV_FORWARD; 121 | if (const_cast(prim).spline.getCurvatureMax() > -0.1 && 122 | const_cast(prim).spline.getCurvatureMax() < 0.1) 123 | { 124 | motion.costMultiplier = mobilityConfig.multiplierForward; 125 | } 126 | else 127 | { 128 | motion.costMultiplier = mobilityConfig.multiplierForwardTurn; 129 | } 130 | break; 131 | case SplinePrimitive::SPLINE_MOVE_BACKWARD: 132 | motion.type = Motion::Type::MOV_BACKWARD; 133 | if (const_cast(prim).spline.getCurvatureMax() > -0.1 && 134 | const_cast(prim).spline.getCurvatureMax() < 0.1) 135 | { 136 | motion.costMultiplier = mobilityConfig.multiplierBackward; 137 | } 138 | else 139 | { 140 | motion.costMultiplier = mobilityConfig.multiplierBackwardTurn; 141 | } 142 | break; 143 | case SplinePrimitive::SPLINE_MOVE_LATERAL: 144 | motion.type = Motion::Type::MOV_LATERAL; 145 | if (const_cast(prim).spline.getCurvatureMax() > -0.1 && 146 | const_cast(prim).spline.getCurvatureMax() < 0.1) 147 | { 148 | motion.costMultiplier = mobilityConfig.multiplierLateral; 149 | } 150 | else 151 | { 152 | motion.costMultiplier = mobilityConfig.multiplierLateralCurve; 153 | } 154 | break; 155 | case SplinePrimitive::SPLINE_POINT_TURN: 156 | motion.type = Motion::Type::MOV_POINTTURN; 157 | motion.costMultiplier = mobilityConfig.multiplierPointTurn; 158 | break; 159 | default: 160 | throw std::runtime_error("Got Unsupported movement"); 161 | } 162 | 163 | //there are no intermediate steps for point turns 164 | if(prim.motionType != SplinePrimitive::SPLINE_POINT_TURN) 165 | { 166 | sampleOnResolution(travGridResolution, prim.spline, motion.intermediateStepsTravMap, motion.fullSplineSamples); 167 | } 168 | computeSplinePrimCost(prim, mobilityConfig, motion); 169 | 170 | if (motion.translationlDist > mobilityConfig.maxMotionCurveLength) //1.3 is slower but trajectories are curvy , 1.0 is faster with more linear trajectories 171 | continue; 172 | 173 | //orientations for backward motions need to be inverted 174 | if(motion.type == Motion::Type::MOV_BACKWARD) 175 | { 176 | for(PoseWithCell& pwc : motion.intermediateStepsTravMap) 177 | pwc.pose.orientation = base::Angle::fromRad(pwc.pose.orientation).flipped().getRad(); 178 | } 179 | 180 | setMotionForTheta(motion, motion.startTheta); 181 | } 182 | } 183 | } 184 | 185 | void PreComputedMotions::setMotionForTheta(const Motion& motion, const DiscreteTheta& theta) 186 | { 187 | if((int)thetaToMotion.size() <= theta.getTheta()) 188 | { 189 | thetaToMotion.resize(theta.getTheta() + 1); 190 | } 191 | 192 | 193 | //check if a motion to this target destination already exist, if yes skip it. 194 | for(const Motion& m : thetaToMotion[theta.getTheta()]) 195 | { 196 | if(m.xDiff == motion.xDiff && m.yDiff == motion.yDiff && 197 | m.endTheta == motion.endTheta && m.type == motion.type) 198 | { 199 | std::string type; 200 | switch(m.type) 201 | { 202 | case Motion::Type::MOV_FORWARD: type ="MOV_FORWARD" ; break; 203 | case Motion::Type::MOV_BACKWARD: type ="MOV_BACKWARD" ; break; 204 | case Motion::Type::MOV_POINTTURN:type ="MOV_POINTTURN" ; break; 205 | case Motion::Type::MOV_LATERAL: type ="MOV_LATERAL" ; break; 206 | default: 207 | throw std::runtime_error("ERROR: motion without valid type: "); 208 | 209 | } 210 | LOG_WARN_S << "WARNING: motion already exists (skipping): " << m.xDiff << ", " << m.yDiff << ", " << m.endTheta << type; 211 | //TODO add check if intermediate poses are similar 212 | return; 213 | } 214 | } 215 | 216 | Motion copy = motion; 217 | copy.id = idToMotion.size(); 218 | 219 | idToMotion.push_back(copy); 220 | thetaToMotion[theta.getTheta()].push_back(copy); 221 | } 222 | 223 | base::Pose2D PreComputedMotions::getPointClosestToCellMiddle(const CellWithPoses& cwp, const double gridResolution) 224 | { 225 | //dummyGrid is used to convert between grid indices and positions 226 | maps::grid::GridMap dummyGrid(maps::grid::Vector2ui(10, 10), base::Vector2d(gridResolution, gridResolution), 0); 227 | 228 | maps::grid::Vector3d cellCenter; 229 | if(!dummyGrid.fromGrid(cwp.cell, cellCenter, 0, false)) 230 | throw std::runtime_error("Internal Error : Cannot convert index to cell position"); 231 | 232 | const maps::grid::Vector2d cellCenter2D = cellCenter.topRows(2); 233 | double closestDist = std::numeric_limits::max(); 234 | assert(cwp.poses.size() > 0); 235 | base::Pose2D closestPose = cwp.poses.front(); 236 | for(const base::Pose2D& pose : cwp.poses) 237 | { 238 | const Eigen::Vector2d centeredPos = pose.position + base::Vector2d(gridResolution /2.0, gridResolution /2.0); 239 | const double dist = (cellCenter2D - centeredPos).norm(); 240 | if(dist < closestDist) 241 | { 242 | closestDist = dist; 243 | closestPose = pose; 244 | } 245 | 246 | maps::grid::Index testIdx; 247 | dummyGrid.toGrid(Eigen::Vector3d(centeredPos.x(), centeredPos.y(), 0), testIdx, false); 248 | 249 | assert(testIdx == cwp.cell); 250 | } 251 | 252 | return closestPose; 253 | } 254 | 255 | 256 | void PreComputedMotions::computeSplinePrimCost(const SplinePrimitive& prim, 257 | const Mobility& mobilityConfig, 258 | Motion& outMotion) const 259 | { 260 | 261 | double linearDist = 0; 262 | double angularDist = 0; 263 | if(prim.motionType == SplinePrimitive::SPLINE_POINT_TURN) 264 | { 265 | angularDist = outMotion.startTheta.shortestDist(outMotion.endTheta).getRadian(); 266 | } 267 | else 268 | { 269 | linearDist = prim.spline.getCurveLength();; 270 | const double stepDist = mobilityConfig.spline_sampling_resolution; 271 | std::vector parameters; 272 | 273 | const std::vector points = prim.spline.sample(stepDist, ¶meters); 274 | assert(parameters.size() == points.size()); 275 | 276 | for(int i = 0; i < ((int)points.size()) - 1; ++i) 277 | { 278 | const double dist = prim.spline.getCurveLength(parameters[i], parameters[i+1], 0.01); 279 | const double curvature = prim.spline.getCurvature(parameters[i]); //assume that the curvature is const between i and i+1 280 | angularDist += dist / linearDist * std::abs(curvature); 281 | } 282 | } 283 | 284 | outMotion.baseCost = Motion::calculateCost(linearDist, angularDist, mobilityConfig.translationSpeed, 285 | mobilityConfig.rotationSpeed, outMotion.costMultiplier); 286 | assert(outMotion.baseCost >= 0); 287 | outMotion.translationlDist = linearDist; 288 | outMotion.angularDist = angularDist; 289 | } 290 | 291 | int Motion::calculateCost(double translationalDist, double angularDist, double translationVelocity, 292 | double angularVelocity, double costMultiplier) 293 | { 294 | if (translationVelocity == 0.0 || angularVelocity == 0.0) { 295 | LOG_ERROR_S << "ERROR calculateCost: Division by zero translation or angular velocity."; 296 | throw std::runtime_error("ERROR calculateCost: Division by zero translation or angular velocity."); 297 | 298 | } 299 | const double translationTime = translationalDist / translationVelocity; 300 | const double angularTime = angularDist / angularVelocity; 301 | 302 | //use ulonglong to catch overflows caused by large cost multipliers 303 | unsigned long long cost = ceil(std::max(angularTime, translationTime) * Motion::costScaleFactor * costMultiplier); 304 | 305 | if(cost > std::numeric_limits::max()) 306 | { 307 | LOG_ERROR_S << "WARNING: primitive cost too large for int. Clipping to int_max."; 308 | return std::numeric_limits::max(); 309 | } 310 | else 311 | return cost; 312 | } 313 | 314 | const Motion& PreComputedMotions::getMotion(std::size_t id) const 315 | { 316 | return idToMotion.at(id); 317 | } 318 | 319 | const SbplSplineMotionPrimitives& PreComputedMotions::getPrimitives() const 320 | { 321 | return primitives; 322 | } 323 | 324 | double PreComputedMotions::calculateCurvatureFromRadius(const double r) 325 | { 326 | assert(r > 0); 327 | /* 328 | Curvature: 329 | c = abs(f''(x)/(1 + f'(x)^2)^(3/2)) 330 | 331 | Circle equation: 332 | f(x) = sqrt(r^2 - x^2) 333 | f'(x) = -x/sqrt(r^2-x^2) 334 | f''(x) = -r^2/((r^2-x^2)^(3/2)) 335 | 336 | Since the curvature of a circle is constant the value of x doesnt matter. 337 | x has to be smaller than r since we calc sqrt(r^2 - x ^2) which is only defined for positive values. 338 | */ 339 | const double x = r/2.0; 340 | const double x2 = x * x; 341 | const double r2 = r * r; 342 | const double df = - (x / std::sqrt(r2 - x2)); 343 | const double ddf = - (r2 / std::pow(r2 - x2, 3.0 / 2.0)); 344 | assert(!std::isnan(df) && !std::isinf(df)); 345 | assert(!std::isnan(ddf) && !std::isinf(ddf)); 346 | 347 | const double df2 = df * df; 348 | const double c = (std::abs(ddf) / std::pow(1 + df2, 3.0 / 2.0)); 349 | 350 | return c; 351 | } 352 | 353 | const std::vector< Motion >& PreComputedMotions::getMotionForStartTheta(const DiscreteTheta& theta) const 354 | { 355 | if(theta.getTheta() >= (int)thetaToMotion.size()) 356 | { 357 | throw std::runtime_error("Internal error, motion for requested theta ist not available. Input theta:" + std::to_string(theta.getTheta())); 358 | } 359 | return thetaToMotion.at(theta.getTheta()); 360 | } 361 | 362 | 363 | double Motion::costScaleFactor = 1000.0; //shift by 3 decimals to keep the decimals as int 364 | 365 | } 366 | -------------------------------------------------------------------------------- /src/gui/PlannerGui.cpp: -------------------------------------------------------------------------------- 1 | #include "PlannerGui.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #ifdef ENABLE_DEBUG_DRAWINGS 22 | #include 23 | #include 24 | #endif 25 | 26 | using namespace ugv_nav4d; 27 | 28 | PlannerGui::PlannerGui(const std::string& dumpName): QObject() 29 | { 30 | setupUI(); 31 | 32 | PlannerDump dump(dumpName); 33 | 34 | mobilityConfig = dump.getMobilityConf(); 35 | travConfig = dump.getTravConfig(); 36 | splineConfig = dump.getSplineConfig(); 37 | plannerConfig = dump.getPlannerConfig(); 38 | 39 | planner.reset(new ugv_nav4d::Planner(splineConfig, travConfig, mobilityConfig, plannerConfig)); 40 | travGen.reset(new traversability_generator3d::TraversabilityGenerator3d(travConfig)); 41 | 42 | sbpl_spline_primitives::SbplSplineMotionPrimitives primitives(splineConfig); 43 | splineViz.setMaxCurvature(ugv_nav4d::PreComputedMotions::calculateCurvatureFromRadius(mobilityConfig.minTurningRadius)); 44 | splineViz.updateData(primitives); 45 | 46 | start = dump.getStart().getPose(); 47 | goal = dump.getGoal().getPose(); 48 | 49 | startViz.updateData(dump.getStart()); 50 | goalViz.updateData(dump.getGoal()); 51 | planner->updateMap(dump.getTravMap()); 52 | trav3dViz.updateData(*(planner->getTraversabilityMap())); 53 | inplanningphase = false; 54 | usingPlannerDump = true; 55 | } 56 | 57 | 58 | PlannerGui::PlannerGui(int argc, char** argv): QObject() 59 | { 60 | usingPlannerDump = false; 61 | setupUI(); 62 | setupPlanner(argc, argv); 63 | } 64 | 65 | void PlannerGui::setupUI() 66 | { 67 | start.orientation.setIdentity(); 68 | goal.orientation.setIdentity(); 69 | 70 | widget = new vizkit3d::Vizkit3DWidget(); 71 | #ifdef ENABLE_DEBUG_DRAWINGS 72 | V3DD::CONFIGURE_DEBUG_DRAWINGS_USE_EXISTING_WIDGET(widget); 73 | #endif 74 | trav3dViz.setPluginName("TravMap"); 75 | 76 | startViz.setPluginName("Start Pose"); 77 | goalViz.setPluginName("Goal Pose"); 78 | 79 | widget->setCameraManipulator(vizkit3d::ORBIT_MANIPULATOR); 80 | widget->addPlugin(&splineViz); 81 | widget->addPlugin(&trajViz); 82 | widget->addPlugin(&trajViz2); 83 | widget->addPlugin(&mlsViz); 84 | widget->addPlugin(&trav3dViz); 85 | widget->addPlugin(&startViz); 86 | widget->addPlugin(&goalViz); 87 | widget->addPlugin(&gridViz); 88 | 89 | splineViz.setPluginEnabled(false); 90 | splineViz.setPluginName("Splines"); 91 | 92 | gridViz.setPluginEnabled(false); 93 | gridViz.setPluginName("Grid"); 94 | 95 | mlsViz.setCycleHeightColor(true); 96 | mlsViz.setShowPatchExtents(false); 97 | mlsViz.setShowNormals(false); 98 | mlsViz.setPluginName("MLSMap"); 99 | 100 | trajViz.setLineWidth(5); 101 | trajViz.setColor(QColor("Cyan")); 102 | trajViz.setPluginEnabled(false); 103 | trajViz.setPluginName("Trajectory 2D"); 104 | 105 | trajViz2.setLineWidth(5); 106 | trajViz2.setColor(QColor("magenta")); 107 | trajViz2.setPluginName("Trajectory 3D"); 108 | 109 | QVBoxLayout* layout = new QVBoxLayout(); 110 | 111 | layout->addWidget(widget); 112 | 113 | maxSlopeSpinBox = new QDoubleSpinBox(); 114 | maxSlopeSpinBox->setMinimum(1); 115 | maxSlopeSpinBox->setMaximum(60); 116 | maxSlopeSpinBox->setValue(33.23); 117 | 118 | connect(maxSlopeSpinBox, SIGNAL(editingFinished()), this, SLOT(maxSlopeEditingFinished())); 119 | QHBoxLayout* slopeLayout = new QHBoxLayout(); 120 | QLabel* lab = new QLabel(); 121 | lab->setText("max slope (deg):"); 122 | 123 | slopeLayout->addWidget(lab); 124 | slopeLayout->addWidget(maxSlopeSpinBox); 125 | //layout->addLayout(slopeLayout); 126 | 127 | QHBoxLayout* timeLayout = new QHBoxLayout(); 128 | time = new QDoubleSpinBox(); 129 | time->setMinimum(1); 130 | time->setMaximum(9999999); 131 | time->setValue(14); 132 | QLabel* lab2 = new QLabel(); 133 | lab2->setText("Max processor time (seconds)"); 134 | timeLayout->addWidget(lab2); 135 | timeLayout->addWidget(time); 136 | timeLayout->addWidget(time); 137 | layout->addLayout(timeLayout); 138 | connect(time, SIGNAL(editingFinished()), this, SLOT(timeEditingFinished())); 139 | 140 | bar = new QProgressBar(); 141 | bar->setMinimum(0); 142 | bar->setMaximum(1); 143 | 144 | inclineLimittingMinSlopeSpinBox = new QDoubleSpinBox(); 145 | inclineLimittingMinSlopeSpinBox->setMinimum(0.0); 146 | inclineLimittingMinSlopeSpinBox->setMaximum(180.0); 147 | inclineLimittingMinSlopeSpinBox->setValue(20); 148 | connect(inclineLimittingMinSlopeSpinBox, SIGNAL(editingFinished()), this, SLOT(inclineLimittingMinSlopeSpinBoxEditingFinished())); 149 | 150 | inclineLimittingLimitSpinBox = new QDoubleSpinBox(); 151 | inclineLimittingLimitSpinBox->setMinimum(0.00001); 152 | inclineLimittingLimitSpinBox->setMaximum(90); 153 | inclineLimittingLimitSpinBox->setValue(25.21); 154 | connect(inclineLimittingLimitSpinBox, SIGNAL(editingFinished()), this, SLOT(inclineLimittingLimitSpinBoxEditingFinished())); 155 | 156 | QLabel* inclineLimittingMinSlopeLabel = new QLabel(); 157 | inclineLimittingMinSlopeLabel->setText("Incline limit min slope (deg)"); 158 | QLabel* inclineLimittingLimitSpinBoxLabel = new QLabel(); 159 | inclineLimittingLimitSpinBoxLabel->setText("Incline limit at max slope (deg)"); 160 | 161 | QHBoxLayout* slopeLimitLayout = new QHBoxLayout(); 162 | slopeLimitLayout->addWidget(inclineLimittingMinSlopeLabel); 163 | slopeLimitLayout->addWidget(inclineLimittingMinSlopeSpinBox); 164 | QHBoxLayout* slopeLimitLayout2 = new QHBoxLayout(); 165 | slopeLimitLayout2->addWidget(inclineLimittingLimitSpinBoxLabel); 166 | slopeLimitLayout2->addWidget(inclineLimittingLimitSpinBox); 167 | 168 | //layout->addLayout(slopeLimitLayout); 169 | //layout->addLayout(slopeLimitLayout2); 170 | 171 | 172 | slopeMetricScaleSpinBox = new QDoubleSpinBox(); 173 | slopeMetricScaleSpinBox->setMinimum(0.0); 174 | slopeMetricScaleSpinBox->setMaximum(999999.0); 175 | slopeMetricScaleSpinBox->setValue(0.0); 176 | connect(slopeMetricScaleSpinBox, SIGNAL(editingFinished()), this, SLOT(slopeMetricScaleSpinBoxEditingFinished())); 177 | 178 | QLabel* slopeMetricLabel = new QLabel(); 179 | slopeMetricLabel->setText("slope metric scale"); 180 | 181 | QHBoxLayout* slopeMetricLayout = new QHBoxLayout(); 182 | slopeMetricLayout->addWidget(slopeMetricLabel); 183 | slopeMetricLayout->addWidget(slopeMetricScaleSpinBox); 184 | //layout->addLayout(slopeMetricLayout); 185 | 186 | slopeMetricComboBox = new QComboBox(); 187 | slopeMetricComboBox->addItem("NONE"); 188 | slopeMetricComboBox->addItem("AVG_SLOPE"); 189 | slopeMetricComboBox->addItem("MAX_SLOPE"); 190 | slopeMetricComboBox->addItem("TRIANGLE_SLOPE"); 191 | connect(slopeMetricComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(slopeMetricComboBoxIndexChanged(int))); 192 | QLabel* slopeMetricComboLabel = new QLabel(); 193 | slopeMetricComboLabel->setText("Slope Metric Type"); 194 | QHBoxLayout* slopeMetricTypeLayout = new QHBoxLayout(); 195 | slopeMetricTypeLayout->addWidget(slopeMetricComboLabel); 196 | slopeMetricTypeLayout->addWidget(slopeMetricComboBox); 197 | //layout->addLayout(slopeMetricTypeLayout); 198 | 199 | startOrientatationSlider = new QSlider(Qt::Horizontal); 200 | startOrientatationSlider->setMinimum(0); 201 | startOrientatationSlider->setMaximum(359); 202 | startOrientatationSlider->setValue(0); 203 | goalOrientationSlider = new QSlider(Qt::Horizontal); 204 | goalOrientationSlider->setMinimum(0); 205 | goalOrientationSlider->setMaximum(359); 206 | goalOrientationSlider->setValue(0); 207 | 208 | connect(startOrientatationSlider, SIGNAL(sliderMoved(int)), this, SLOT(startOrientationChanged(int))); 209 | connect(goalOrientationSlider, SIGNAL(sliderMoved(int)), this, SLOT(goalOrientationChanged(int))); 210 | 211 | QLabel* startOrientationLaebel = new QLabel(); 212 | startOrientationLaebel->setText("Start orientation (deg)"); 213 | QLabel* goalOrientationLaebel = new QLabel(); 214 | goalOrientationLaebel->setText("Goal orientation (deg)"); 215 | 216 | QHBoxLayout* startOrientationLayout = new QHBoxLayout(); 217 | startOrientationLayout->addWidget(startOrientationLaebel); 218 | startOrientationLayout->addWidget(startOrientatationSlider); 219 | 220 | QHBoxLayout* goalOrientationLayout = new QHBoxLayout(); 221 | goalOrientationLayout->addWidget(goalOrientationLaebel); 222 | goalOrientationLayout->addWidget(goalOrientationSlider); 223 | 224 | layout->addLayout(startOrientationLayout); 225 | layout->addLayout(goalOrientationLayout); 226 | 227 | obstacleDistanceSpinBox = new QDoubleSpinBox(); 228 | obstacleDistanceSpinBox->setMaximum(99999); 229 | obstacleDistanceSpinBox->setMinimum(0); 230 | obstacleDistanceSpinBox->setValue(0.0); 231 | 232 | obstacleFactorSpinBox = new QDoubleSpinBox(); 233 | obstacleFactorSpinBox->setMinimum(0); 234 | obstacleFactorSpinBox->setMaximum(9999999); 235 | obstacleFactorSpinBox->setValue(1.0); 236 | 237 | QLabel* obstacleDistanceLable = new QLabel(); 238 | obstacleDistanceLable->setText("Obstacle Distance"); 239 | QHBoxLayout* obstacleDistLayout = new QHBoxLayout(); 240 | obstacleDistLayout->addWidget(obstacleDistanceLable); 241 | obstacleDistLayout->addWidget(obstacleDistanceSpinBox); 242 | 243 | QLabel* obstacleFactorLabel = new QLabel(); 244 | obstacleFactorLabel->setText("Obstacle cost factor"); 245 | QHBoxLayout* obstacleFactorLayout = new QHBoxLayout(); 246 | obstacleFactorLayout->addWidget(obstacleFactorLabel); 247 | obstacleFactorLayout->addWidget(obstacleFactorSpinBox); 248 | 249 | //layout->addLayout(obstacleDistLayout); 250 | //layout->addLayout(obstacleFactorLayout); 251 | 252 | connect(obstacleDistanceSpinBox, SIGNAL(editingFinished()), this, SLOT(obstacleDistanceSpinBoxEditingFinished())); 253 | connect(obstacleFactorSpinBox, SIGNAL(editingFinished()), this, SLOT(obstacleFactorSpinBoxEditingFinished())); 254 | 255 | numThreadsSpinBox = new QSpinBox(); 256 | numThreadsSpinBox->setValue(4); 257 | QLabel* parallelismLabel = new QLabel(); 258 | parallelismLabel->setText("Number of Threads"); 259 | 260 | QHBoxLayout* parallelismLayout = new QHBoxLayout(); 261 | parallelismLayout->addWidget(parallelismLabel); 262 | parallelismLayout->addWidget(numThreadsSpinBox); 263 | 264 | layout->addLayout(parallelismLayout); 265 | connect(numThreadsSpinBox, SIGNAL(valueChanged(int)), this, SLOT(numThreadsValueChanged(int))); 266 | 267 | 268 | QPushButton* replanButton = new QPushButton("Plan"); 269 | timeLayout->addWidget(replanButton); 270 | 271 | QPushButton* dumpButton = new QPushButton("Create PlannerDump"); 272 | timeLayout->addWidget(dumpButton); 273 | 274 | 275 | connect(replanButton, SIGNAL(released()), this, SLOT(replanButtonReleased())); 276 | connect(dumpButton, SIGNAL(released()), this, SLOT(dumpPressed())); 277 | 278 | 279 | layout->addWidget(bar); 280 | 281 | window.setLayout(layout); 282 | 283 | //to be able to send Trajectory via slot 284 | qRegisterMetaType>("std::vector"); 285 | qRegisterMetaType>("std::vector"); 286 | qRegisterMetaType>("maps::grid::TraversabilityMap3d< maps::grid::TraversabilityNodeBase*>"); 287 | 288 | connect(&mlsViz, SIGNAL(picked(float,float,float, int, int)), this, SLOT(picked(float,float,float, int, int))); 289 | connect(&trav3dViz, SIGNAL(picked(float,float,float, int, int)), this, SLOT(picked(float,float,float, int, int))); 290 | connect(this, SIGNAL(plannerDone()), this, SLOT(plannerIsDone())); 291 | 292 | } 293 | 294 | 295 | void PlannerGui::setupPlanner(int argc, char** argv) 296 | { 297 | double res = 0.3; 298 | if(argc > 2){ 299 | std::setlocale(LC_ALL, "C"); 300 | res = atof(argv[2]); 301 | } 302 | 303 | splineConfig.gridSize = res; 304 | splineConfig.numAngles = 16; 305 | splineConfig.numEndAngles = 8; 306 | splineConfig.destinationCircleRadius = 6; 307 | splineConfig.cellSkipFactor = 0.1; 308 | splineConfig.generatePointTurnMotions = true; 309 | splineConfig.generateLateralMotions = true; 310 | splineConfig.generateBackwardMotions = true; 311 | splineConfig.generateForwardMotions = true; 312 | splineConfig.splineOrder = 4; 313 | 314 | mobilityConfig.translationSpeed = 0.5; 315 | mobilityConfig.rotationSpeed = 0.5; 316 | mobilityConfig.minTurningRadius = 1; // increase this to reduce the number of available motion primitives 317 | mobilityConfig.searchRadius = 0.0; 318 | mobilityConfig.searchProgressSteps = 0.1; 319 | mobilityConfig.multiplierForward = 1; 320 | mobilityConfig.multiplierForwardTurn = 2; 321 | mobilityConfig.multiplierBackward = 2; 322 | mobilityConfig.multiplierBackwardTurn = 3; 323 | mobilityConfig.multiplierLateral = 4; 324 | mobilityConfig.multiplierLateralCurve = 4; 325 | mobilityConfig.multiplierPointTurn = 3; 326 | mobilityConfig.maxMotionCurveLength = 100; 327 | mobilityConfig.spline_sampling_resolution = 0.05; 328 | mobilityConfig.remove_goal_offset = false; 329 | 330 | travConfig.gridResolution = res; 331 | travConfig.maxSlope = 0.45; //40.0/180.0 * M_PI; 332 | travConfig.maxStepHeight = 0.25; //space below robot 333 | travConfig.robotSizeX = 0.5; 334 | travConfig.robotSizeY = 0.5; 335 | travConfig.robotHeight = 0.5; //incl space below body 336 | travConfig.slopeMetricScale = 1.0; 337 | travConfig.slopeMetric = traversability_generator3d::SlopeMetric::NONE; 338 | travConfig.inclineLimittingMinSlope = 0.22; // 10.0 * M_PI/180.0; 339 | travConfig.inclineLimittingLimit = 0.43;// 5.0 * M_PI/180.0; 340 | travConfig.costFunctionDist = 0.0; 341 | travConfig.distToGround = 0.0; 342 | travConfig.minTraversablePercentage = 0.5; 343 | travConfig.allowForwardDownhill = true; 344 | travConfig.enableInclineLimitting = false; 345 | 346 | plannerConfig.epsilonSteps = 2.0; 347 | plannerConfig.initialEpsilon = 64.0; 348 | plannerConfig.numThreads = 4; 349 | 350 | planner.reset(new ugv_nav4d::Planner(splineConfig, travConfig, mobilityConfig, plannerConfig)); 351 | travGen.reset(new traversability_generator3d::TraversabilityGenerator3d(travConfig)); 352 | 353 | sbpl_spline_primitives::SbplSplineMotionPrimitives primitives(splineConfig); 354 | 355 | splineViz.setMaxCurvature(ugv_nav4d::PreComputedMotions::calculateCurvatureFromRadius(mobilityConfig.minTurningRadius)); 356 | splineViz.updateData(primitives); 357 | 358 | if(argc > 1) 359 | { 360 | const std::string mls(argv[1]); 361 | loadMls(mls); 362 | } 363 | else 364 | { 365 | loadMls(); 366 | } 367 | 368 | if(argc > 3) 369 | { 370 | start.position << 9.08776, 5.8535, 0.06052; 371 | QVector3D pos(start.position.x(), start.position.y(), start.position.z()); 372 | startViz.setTranslation(pos); 373 | 374 | 375 | LOG_INFO_S << "Start: " << start.position.transpose(); 376 | pickStart = false; 377 | } 378 | } 379 | 380 | 381 | void PlannerGui::loadMls() 382 | { 383 | const QString file = QFileDialog::getOpenFileName(nullptr, tr("Load mls map"), 384 | QDir::currentPath(), QString(), 385 | nullptr, QFileDialog::DontUseNativeDialog); 386 | if(!file.isEmpty()) 387 | { 388 | loadMls(file.toStdString()); 389 | } 390 | } 391 | 392 | void PlannerGui::loadMls(const std::string& path) 393 | { 394 | std::ifstream fileIn(path); 395 | 396 | 397 | if(path.find(".ply") != std::string::npos) 398 | { 399 | LOG_INFO_S << "Loading PLY"; 400 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 401 | pcl::PLYReader plyReader; 402 | if(plyReader.read(path, *cloud) >= 0) 403 | { 404 | pcl::PointXYZ mi, ma; 405 | pcl::getMinMax3D (*cloud, mi, ma); 406 | LOG_INFO_S << "MIN: " << mi << ", MAX: " << ma; 407 | 408 | const double mls_res = travConfig.gridResolution; 409 | const double size_x = ma.x - mi.x; 410 | const double size_y = ma.y - mi.y; 411 | 412 | const maps::grid::Vector2ui numCells(size_x / mls_res + 2, size_y / mls_res + 2); 413 | LOG_INFO_S << "NUM CELLS: " << numCells.transpose(); 414 | 415 | maps::grid::MLSConfig cfg; 416 | cfg.gapSize = 0.1; 417 | const maps::grid::Vector2d mapSize(numCells[0]*mls_res, numCells[1]*mls_res); 418 | const maps::grid::Vector3d offset(mi.x-0.5*mls_res, mi.y-0.5*mls_res, 0); 419 | LOG_DEBUG_S << "Range(x): [" << offset[0] << "; " << mapSize[0]+offset[0] << "], " 420 | << "Range(y): [" << offset[1] << "; " << mapSize[1]+offset[1] << "]\n"; 421 | 422 | mlsMap = maps::grid::MLSMapSloped(numCells, maps::grid::Vector2d(mls_res, mls_res), cfg); 423 | mlsMap.translate(offset); 424 | mlsMap.mergePointCloud(*cloud, base::Transform3d::Identity()); 425 | mlsViz.updateMLSSloped(mlsMap); 426 | 427 | std::shared_ptr mlsPtr = std::make_shared(mlsMap); 428 | travGen->setMLSGrid(mlsPtr); 429 | } 430 | return; 431 | } 432 | try 433 | { 434 | LOG_INFO_S << "Loading MLS"; 435 | boost::archive::binary_iarchive mlsIn(fileIn); 436 | mlsIn >> mlsMap; 437 | mlsViz.updateMLSSloped(mlsMap); 438 | 439 | std::shared_ptr mlsPtr = std::make_shared(mlsMap); 440 | travGen->setMLSGrid(mlsPtr); 441 | return; 442 | } 443 | catch(...) {} 444 | 445 | 446 | std::cerr << "Unabled to load mls. Unknown format"; 447 | 448 | } 449 | 450 | 451 | void PlannerGui::picked(float x, float y, float z, int buttonMask, int modifierMask) 452 | { 453 | // start << 5.91327, 1.38306, -1.39575; 454 | // goal << 7.47328, 1.34183, -1.39437; 455 | // startPlanThread(); 456 | 457 | //1 = left click 458 | //4 = right click 459 | 460 | switch(buttonMask) 461 | { 462 | case 1: //left click 463 | { 464 | start.position << x, y, z; 465 | start.position.z() += travConfig.distToGround; //because we click on the ground but need to put robot position 466 | 467 | #ifdef ENABLE_DEBUG_DRAWINGS 468 | V3DD::CLEAR_DRAWING("ugv_nav4d_start_aabb"); 469 | V3DD::DRAW_WIREFRAME_BOX("ugv_nav4d_start_aabb", start.position + base::Vector3d(0, 0, travConfig.distToGround / 2.0), start.orientation, 470 | base::Vector3d(travConfig.robotSizeX, travConfig.robotSizeY, travConfig.robotHeight - travConfig.distToGround), V3DD::Color::cyan); 471 | #endif 472 | QVector3D pos(start.position.x(), start.position.y(), start.position.z()); 473 | startViz.setTranslation(pos); 474 | LOG_INFO_S << "Start: " << start.position.transpose(); 475 | startPicked = true; 476 | } 477 | break; 478 | case 4: //right click 479 | { 480 | goal.position << x, y, z; 481 | goal.position.z() += travConfig.distToGround; 482 | QVector3D pos(goal.position.x(), goal.position.y(), goal.position.z()); 483 | goalViz.setTranslation(pos); 484 | LOG_INFO_S << "goal: " << goal.position.transpose(); 485 | goalPicked = true; 486 | } 487 | break; 488 | default: 489 | break; 490 | } 491 | } 492 | 493 | void PlannerGui::show() 494 | { 495 | window.show(); 496 | } 497 | 498 | void PlannerGui::maxSlopeEditingFinished() 499 | { 500 | travConfig.maxSlope = maxSlopeSpinBox->value()/180.0 * M_PI; 501 | } 502 | 503 | void PlannerGui::inclineLimittingLimitSpinBoxEditingFinished() 504 | { 505 | travConfig.inclineLimittingLimit = inclineLimittingLimitSpinBox->value()/180.0 * M_PI; 506 | } 507 | 508 | void PlannerGui::inclineLimittingMinSlopeSpinBoxEditingFinished() 509 | { 510 | travConfig.inclineLimittingMinSlope = inclineLimittingMinSlopeSpinBox->value()/180.0 * M_PI; 511 | } 512 | 513 | void PlannerGui::slopeMetricScaleSpinBoxEditingFinished() 514 | { 515 | travConfig.slopeMetricScale = slopeMetricScaleSpinBox->value(); 516 | } 517 | 518 | void PlannerGui::slopeMetricComboBoxIndexChanged(int index) 519 | { 520 | std::vector metrics = {traversability_generator3d::SlopeMetric::NONE, 521 | traversability_generator3d::SlopeMetric::AVG_SLOPE, 522 | traversability_generator3d::SlopeMetric::MAX_SLOPE, 523 | traversability_generator3d::SlopeMetric::TRIANGLE_SLOPE}; 524 | if(size_t(index) < metrics.size()) 525 | { 526 | travConfig.slopeMetric = metrics[index]; 527 | } 528 | else 529 | { 530 | throw std::runtime_error("unknown slope index"); 531 | } 532 | } 533 | 534 | void PlannerGui::numThreadsValueChanged(int newValue) 535 | { 536 | if(newValue >= 1) 537 | { 538 | plannerConfig.numThreads = newValue; 539 | } 540 | } 541 | 542 | 543 | void PlannerGui::goalOrientationChanged(int newValue) 544 | { 545 | const double rad = newValue/180.0 * M_PI; 546 | goal.orientation = Eigen::AngleAxisd(rad, Eigen::Vector3d::UnitZ()); 547 | goalViz.setRotation(QQuaternion(goal.orientation.w(), goal.orientation.x(), goal.orientation.y(), goal.orientation.z())); 548 | } 549 | 550 | void PlannerGui::startOrientationChanged(int newValue) 551 | { 552 | const double rad = newValue/180.0 * M_PI; 553 | start.orientation = Eigen::AngleAxisd(rad, Eigen::Vector3d::UnitZ()); 554 | startViz.setRotation(QQuaternion(start.orientation.w(), start.orientation.x(), start.orientation.y(), start.orientation.z())); 555 | #ifdef ENABLE_DEBUG_DRAWINGS 556 | V3DD::CLEAR_DRAWING("ugv_nav4d_start_aabb"); 557 | V3DD::DRAW_WIREFRAME_BOX("ugv_nav4d_start_aabb", start.position + Eigen::Vector3d(0, 0, travConfig.distToGround), 558 | start.orientation, base::Vector3d(travConfig.robotSizeX, travConfig.robotSizeY, travConfig.robotHeight), V3DD::Color::cyan); 559 | #endif 560 | } 561 | 562 | void PlannerGui::obstacleDistanceSpinBoxEditingFinished() 563 | { 564 | travConfig.costFunctionDist = obstacleDistanceSpinBox->value(); 565 | } 566 | 567 | void PlannerGui::obstacleFactorSpinBoxEditingFinished() 568 | { 569 | throw std::runtime_error("Function removed"); 570 | // travConfig.costFunctionObstacleMultiplier = obstacleFactorSpinBox->value(); 571 | } 572 | 573 | void PlannerGui::timeEditingFinished() 574 | { 575 | 576 | } 577 | 578 | void PlannerGui::replanButtonReleased() 579 | { 580 | planner->setTravConfig(travConfig); 581 | planner->setPlannerConfig(plannerConfig); 582 | startPlanThread(); 583 | } 584 | 585 | void PlannerGui::startPlanThread() 586 | { 587 | 588 | bar->setMaximum(0); 589 | 590 | // Check if planning is already in progress 591 | if (inplanningphase.load()) { 592 | std::cout << "Planner is in planning phase... Please wait for it to finish." << std::endl; 593 | return; 594 | } 595 | 596 | // Mark the start of the planning phase 597 | inplanningphase.store(true); 598 | 599 | std::thread t([this](){ 600 | #ifdef ENABLE_DEBUG_DRAWINGS 601 | V3DD::CONFIGURE_DEBUG_DRAWINGS_USE_EXISTING_WIDGET(this->widget); 602 | #endif 603 | if (!usingPlannerDump){ 604 | std::vector startPositions; 605 | startPositions.emplace_back(Eigen::Vector3d(this->start.position.x(), 606 | this->start.position.y(), 607 | this->start.position.z()-travConfig.distToGround)); 608 | 609 | travGen->expandAll(startPositions); 610 | planner->updateMap(travGen->getTraversabilityMap()); 611 | } 612 | this->plan(this->start, this->goal); 613 | 614 | // Mark the end of the planning phase after work is done 615 | inplanningphase.store(false); 616 | 617 | }); 618 | t.detach(); //needed to avoid destruction of thread at end of method 619 | } 620 | 621 | 622 | void PlannerGui::plannerIsDone() 623 | { 624 | trajViz.updateData(path); 625 | trajViz.setLineWidth(8); 626 | 627 | trajViz2.updateData(beautifiedPath); 628 | trajViz2.setLineWidth(8); 629 | 630 | trav3dViz.updateData(*(planner->getTraversabilityMap())); 631 | 632 | bar->setMaximum(1); 633 | } 634 | 635 | void PlannerGui::dumpPressed() 636 | { 637 | LOG_INFO_S << "Dumping"; 638 | 639 | base::samples::RigidBodyState startState; 640 | startState.position = start.position; 641 | startState.orientation = start.orientation; 642 | base::samples::RigidBodyState endState; 643 | endState.position << goal.position; 644 | endState.orientation = goal.orientation; 645 | 646 | PlannerDump dump(*planner, "created_by_test_gui", base::Time::fromSeconds(time->value()), 647 | startState, endState); 648 | } 649 | 650 | 651 | 652 | /* 653 | Start: 5.99972 0.399847 -1.31341d 654 | goal: -0.455198 7.99133 2.08586 655 | */ 656 | 657 | void PlannerGui::plan(const base::Pose& start, const base::Pose& goal) 658 | { 659 | base::samples::RigidBodyState startState; 660 | startState.position = start.position; 661 | startState.orientation = start.orientation; 662 | base::samples::RigidBodyState endState; 663 | endState.position << goal.position; 664 | endState.orientation = goal.orientation; 665 | 666 | LOG_INFO_S << "Planning: " << start << " -> " << goal; 667 | 668 | const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(time->value()), 669 | startState, endState, path, beautifiedPath); 670 | switch(result) 671 | { 672 | case Planner::GOAL_INVALID: 673 | LOG_INFO_S << "GOAL_INVALID"; 674 | break; 675 | case Planner::START_INVALID: 676 | LOG_INFO_S << "START_INVALID"; 677 | break; 678 | case Planner::NO_SOLUTION: 679 | LOG_INFO_S << "NO_SOLUTION"; 680 | break; 681 | case Planner::NO_MAP: 682 | LOG_INFO_S << "NO_MAP"; 683 | break; 684 | case Planner::INTERNAL_ERROR: 685 | LOG_INFO_S << "INTERNAL_ERROR"; 686 | break; 687 | case Planner::FOUND_SOLUTION: 688 | LOG_INFO_S << "FOUND_SOLUTION"; 689 | break; 690 | default: 691 | LOG_INFO_S << "ERROR unknown result state"; 692 | break; 693 | } 694 | 695 | emit plannerDone(); 696 | } 697 | 698 | 699 | 700 | 701 | --------------------------------------------------------------------------------