├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md └── workflows │ ├── build_and_install.yaml │ ├── build_and_install_manual_deps.yaml │ ├── build_and_install_within_catkin.yaml │ ├── build_and_install_within_catkin_vcstool.yaml │ ├── clang-format.yaml │ └── run_tests.yaml ├── .gitignore ├── .pre-commit-config.yaml ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── deps.repos ├── docs ├── graph_core_logo.png ├── graph_core_logo_background.png ├── graph_core_logo_blue.png └── tutorial │ └── tutorial_intro.md ├── graph_core ├── .clang-format ├── CMakeLists.txt ├── cmake_config │ ├── Dependencies.cmake │ ├── clang_format_target.sh │ └── graph_coreConfig.cmake.in ├── include │ └── graph_core │ │ ├── collision_checkers │ │ ├── collision_checker_base.h │ │ └── cube_3d_collision_checker.h │ │ ├── datastructure │ │ ├── kdtree.h │ │ ├── nearest_neighbors.h │ │ └── vector.h │ │ ├── graph │ │ ├── connection.h │ │ ├── net.h │ │ ├── node.h │ │ ├── path.h │ │ ├── subtree.h │ │ └── tree.h │ │ ├── metrics │ │ ├── euclidean_metrics.h │ │ ├── goal_cost_function_base.h │ │ ├── hamp_goal_cost_function_base.h │ │ ├── hamp_metrics_base.h │ │ ├── metrics_base.h │ │ └── null_goal_cost_function.h │ │ ├── plugins │ │ ├── collision_checkers │ │ │ └── collision_checker_base_plugin.h │ │ ├── metrics │ │ │ ├── euclidean_metrics_plugin.h │ │ │ ├── goal_cost_function_base_plugin.h │ │ │ ├── hamp_goal_cost_function_base_plugin.h │ │ │ ├── hamp_metrics_base_plugin.h │ │ │ ├── metrics_base_plugin.h │ │ │ └── null_goal_cost_function_plugin.h │ │ ├── samplers │ │ │ ├── ball_sampler_plugin.h │ │ │ ├── informed_sampler_plugin.h │ │ │ ├── sampler_base_plugin.h │ │ │ └── uniform_sampler_plugin.h │ │ └── solvers │ │ │ ├── anytime_rrt_plugin.h │ │ │ ├── birrt_plugin.h │ │ │ ├── rrt_plugin.h │ │ │ ├── rrt_star_plugin.h │ │ │ └── tree_solver_plugin.h │ │ ├── samplers │ │ ├── ball_sampler.h │ │ ├── informed_sampler.h │ │ ├── sampler_base.h │ │ ├── tube_informed_sampler.h │ │ └── uniform_sampler.h │ │ ├── solvers │ │ ├── anytime_rrt.h │ │ ├── birrt.h │ │ ├── path_optimizers │ │ │ ├── path_local_optimizer.h │ │ │ └── path_optimizer_base.h │ │ ├── rrt.h │ │ ├── rrt_star.h │ │ └── tree_solver.h │ │ └── util.h ├── package.xml ├── src │ └── graph_core │ │ ├── datastructure │ │ ├── kdtree.cpp │ │ └── vector.cpp │ │ ├── graph │ │ ├── connection.cpp │ │ ├── net.cpp │ │ ├── node.cpp │ │ ├── path.cpp │ │ ├── subtree.cpp │ │ └── tree.cpp │ │ ├── metrics │ │ └── euclidean_metrics.cpp │ │ ├── plugins │ │ ├── metrics │ │ │ ├── euclidean_metrics_plugin.cpp │ │ │ └── null_goal_cost_function_plugin.cpp │ │ ├── samplers │ │ │ ├── ball_sampler_plugin.cpp │ │ │ ├── informed_sampler_plugin.cpp │ │ │ └── uniform_sampler_plugin.cpp │ │ └── solvers │ │ │ ├── anytime_rrt_plugin.cpp │ │ │ ├── birrt_plugin.cpp │ │ │ ├── rrt_plugin.cpp │ │ │ └── rrt_star_plugin.cpp │ │ ├── samplers │ │ ├── ball_sampler.cpp │ │ ├── informed_sampler.cpp │ │ ├── tube_informed_sampler.cpp │ │ └── uniform_sampler.cpp │ │ └── solvers │ │ ├── anytime_rrt.cpp │ │ ├── birrt.cpp │ │ ├── path_optimizers │ │ ├── path_local_optimizer.cpp │ │ └── path_optimizer_base.cpp │ │ ├── rrt.cpp │ │ ├── rrt_star.cpp │ │ └── tree_solver.cpp └── tests │ ├── CMakeLists.txt │ ├── graph_core_tests_params.yaml │ ├── logger_param.yaml │ ├── logger_param_gtest.yaml │ ├── path.yaml │ ├── src │ ├── compute_path_test.cpp │ ├── kdtree_test.cpp │ ├── node_connection_test.cpp │ ├── path_post_processing_test.cpp │ └── run_all_tests.cpp │ └── tree.yaml ├── hooks └── pre-commit-clang-format └── pull_request_template.md /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: bug 6 | assignees: CesareTonola 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Provide the full code that reproduces the error and explain how to run it. E.g., provide the robotic cell used for the test. 15 | 16 | **Expected behavior** 17 | A clear and concise description of what you expected to happen. 18 | 19 | **Screenshots** 20 | If applicable, add screenshots to help explain your problem. 21 | 22 | **Additional context** 23 | Add any other context about the problem here. 24 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: enhancement 6 | assignees: CesareTonola 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.github/workflows/build_and_install.yaml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | - devel 8 | - pull_req 9 | pull_request: 10 | branches: 11 | - master 12 | schedule: 13 | - cron: '0 8 * * *' 14 | 15 | jobs: 16 | install-and-run: 17 | strategy: 18 | matrix: 19 | ubuntu_version: [ubuntu:20.04, ubuntu:22.04, ubuntu:latest] 20 | fail-fast: true 21 | runs-on: ubuntu-latest 22 | container: 23 | image: ${{ matrix.ubuntu_version }} 24 | 25 | steps: 26 | - name: Checkout repository 27 | uses: actions/checkout@v4 28 | 29 | - name: Install dependencies 30 | run: | 31 | export DEBIAN_FRONTEND=noninteractive 32 | apt update 33 | apt install -y cmake build-essential git libboost-all-dev libeigen3-dev libyaml-cpp-dev libpoco-dev liblog4cxx-dev libgtest-dev 34 | 35 | - name: Build & Install 36 | run: | 37 | mkdir -p build 38 | mkdir -p install 39 | 40 | export PATH_TO_WS="$(pwd)" 41 | 42 | echo "Workspace Path: $PATH_TO_WS" 43 | ls -la 44 | 45 | export PATH="$PATH_TO_WS/install/bin:$PATH" 46 | export LD_LIBRARY_PATH="$PATH_TO_WS/install/lib" 47 | export CMAKE_PREFIX_PATH="$PATH_TO_WS/install" 48 | 49 | echo "PATH: $PATH" 50 | echo "LD_LIBRARY_PATH: $LD_LIBRARY_PATH" 51 | echo "CMAKE_PREFIX_PATH: $CMAKE_PREFIX_PATH" 52 | 53 | mkdir -p build/graph_core 54 | cmake -S graph_core -B build/graph_core -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install 55 | make -C build/graph_core install 56 | -------------------------------------------------------------------------------- /.github/workflows/build_and_install_manual_deps.yaml: -------------------------------------------------------------------------------- 1 | name: build-manual-deps 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | - devel 8 | - pull_req 9 | pull_request: 10 | branches: 11 | - master 12 | schedule: 13 | - cron: '0 8 * * *' 14 | 15 | jobs: 16 | install-and-run: 17 | strategy: 18 | matrix: 19 | ubuntu_version: [ubuntu:20.04, ubuntu:22.04, ubuntu:latest] 20 | fail-fast: true 21 | runs-on: ubuntu-latest 22 | container: 23 | image: ${{ matrix.ubuntu_version }} 24 | 25 | steps: 26 | - name: Checkout repository 27 | uses: actions/checkout@v4 28 | 29 | - name: Install dependencies 30 | run: | 31 | export DEBIAN_FRONTEND=noninteractive 32 | apt update 33 | apt install -y cmake build-essential git libboost-all-dev libeigen3-dev libyaml-cpp-dev libpoco-dev liblog4cxx-dev libgtest-dev 34 | 35 | - name: Build & Install cnr_common 36 | run: | 37 | mkdir -p build 38 | mkdir -p install 39 | 40 | export PATH_TO_WS="$(pwd)" 41 | echo "PATH_TO_WS=$PATH_TO_WS" >> $GITHUB_ENV 42 | echo "Workspace Path: $PATH_TO_WS" 43 | ls -la 44 | 45 | export PATH="$PATH_TO_WS/install/bin:$PATH" 46 | export LD_LIBRARY_PATH="$PATH_TO_WS/install/lib" 47 | export CMAKE_PREFIX_PATH="$PATH_TO_WS/install" 48 | 49 | echo "PATH=$PATH_TO_WS/install/bin:$PATH" >> "$GITHUB_ENV" 50 | echo "LD_LIBRARY_PATH=$PATH_TO_WS/install/lib" >> "$GITHUB_ENV" 51 | echo "CMAKE_PREFIX_PATH=$PATH_TO_WS/install" >> "$GITHUB_ENV" 52 | 53 | echo "PATH: $PATH" 54 | echo "LD_LIBRARY_PATH: $LD_LIBRARY_PATH" 55 | echo "CMAKE_PREFIX_PATH: $CMAKE_PREFIX_PATH" 56 | 57 | git clone --recurse-submodules https://github.com/JRL-CARI-CNR-UNIBS/cnr_common.git 58 | 59 | cd cnr_common 60 | git submodule update --init --recursive 61 | chmod +x update_submodules.sh 62 | ls -la 63 | ./update_submodules.sh 64 | 65 | cd "$PATH_TO_WS" 66 | mkdir -p build/cnr_logger 67 | cmake -S cnr_common/cnr_logger -B build/cnr_logger -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install -DUSE_ROS1=OFF -DCOMPILE_EXAMPLE=OFF -DENABLE_TESTING=OFF-DENABLE_COVERAGE_TESTING=OFF 68 | make -C build/cnr_logger install 69 | 70 | mkdir -p build/cnr_yaml 71 | cmake -S cnr_common/cnr_yaml -B build/cnr_yaml -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install -DBUILD_UNIT_TESTS=OFF 72 | make -C build/cnr_yaml install 73 | 74 | mkdir -p build/cnr_param 75 | cmake -S cnr_common/cnr_param -B build/cnr_param -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install -DCOMPILE_MAPPED_FILE_MODULE=ON -DBUILD_UNIT_TESTS=OFF -DBUILD_INTEGRATION_TESTS=OFF -DRETRIVE_DEPENDENCIES=OFF -DBUILD_INTEGRATION_TESTS=OFF -DBUILD_AS_A_CATKIN_PACKAGE=OFF 76 | make -C build/cnr_param install 77 | 78 | mkdir -p build/cnr_class_loader 79 | cmake -S cnr_common/cnr_class_loader -B build/cnr_class_loader -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install 80 | make -C build/cnr_class_loader install 81 | 82 | - name: Build & Install graph_core 83 | run: | 84 | mkdir -p build/graph_core 85 | cmake -S graph_core -B build/graph_core -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install 86 | make -C build/graph_core install 87 | -------------------------------------------------------------------------------- /.github/workflows/build_and_install_within_catkin.yaml: -------------------------------------------------------------------------------- 1 | name: build-within-catkin 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | - devel 8 | - pull_req 9 | pull_request: 10 | branches: 11 | - master 12 | schedule: 13 | - cron: '0 8 * * *' 14 | 15 | jobs: 16 | install-and-run-within-catkin: 17 | runs-on: ubuntu-latest 18 | container: 19 | image: osrf/ros:noetic-desktop-full 20 | 21 | steps: 22 | - name: Install dependencies 23 | run: | 24 | sudo apt update 25 | sudo apt install -y git python3-catkin-tools libboost-all-dev libeigen3-dev libyaml-cpp-dev libpoco-dev liblog4cxx-dev libgtest-dev 26 | 27 | - name: Setup Catkin Workspace 28 | shell: bash 29 | run: | 30 | echo "Setting up ROS environment" 31 | source /opt/ros/noetic/setup.bash 32 | WORKSPACE_DIR=$(pwd)/graph_core_ws 33 | echo "WORKSPACE_DIR=$(pwd)/graph_core_ws" >> "$GITHUB_ENV" 34 | echo "Setting up Catkin workspace at $WORKSPACE_DIR" 35 | mkdir -p $WORKSPACE_DIR/src 36 | cd $WORKSPACE_DIR 37 | catkin init 38 | catkin config --extend /opt/ros/noetic 39 | catkin config --install 40 | ls -la 41 | 42 | - name: Checkout repository 43 | uses: actions/checkout@v4 44 | with: 45 | path: "${{env.WORKSPACE_DIR }}/src/graph_core" 46 | 47 | - name: Build & Install graph_core in Catkin Workspace 48 | shell: bash 49 | run: | 50 | cd ${{env.WORKSPACE_DIR }} 51 | catkin build -cs 52 | source install/setup.bash 53 | -------------------------------------------------------------------------------- /.github/workflows/build_and_install_within_catkin_vcstool.yaml: -------------------------------------------------------------------------------- 1 | name: build-within-catkin-vcstool 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | - devel 8 | - pull_req 9 | pull_request: 10 | branches: 11 | - master 12 | schedule: 13 | - cron: '0 8 * * *' 14 | 15 | jobs: 16 | install-and-run-within-catkin: 17 | runs-on: ubuntu-latest 18 | container: 19 | image: osrf/ros:noetic-desktop-full 20 | 21 | steps: 22 | - name: Install dependencies 23 | run: | 24 | sudo apt update 25 | sudo apt install -y git python3-catkin-tools libboost-all-dev libeigen3-dev libyaml-cpp-dev libpoco-dev liblog4cxx-dev libgtest-dev python3-vcstool 26 | 27 | - name: Setup Catkin Workspace 28 | shell: bash 29 | run: | 30 | echo "Setting up ROS environment" 31 | source /opt/ros/noetic/setup.bash 32 | 33 | WORKSPACE_DIR=$(pwd)/graph_core_ws 34 | 35 | echo "WORKSPACE_DIR=$(pwd)/graph_core_ws" >> "$GITHUB_ENV" 36 | 37 | echo "Setting up Catkin workspace at $WORKSPACE_DIR" 38 | mkdir -p $WORKSPACE_DIR/src 39 | cd $WORKSPACE_DIR 40 | 41 | catkin init 42 | catkin config --extend /opt/ros/noetic 43 | catkin config --install 44 | 45 | ls -la 46 | 47 | - name: Checkout repository 48 | uses: actions/checkout@v4 49 | with: 50 | path: "${{env.WORKSPACE_DIR }}/src/graph_core" 51 | 52 | - name: Download deps with vcstool 53 | shell: bash 54 | run: | 55 | cd ${{env.WORKSPACE_DIR }}/src 56 | vcs import < graph_core/deps.repos 57 | 58 | - name: Build & Install graph_core in Catkin Workspace 59 | shell: bash 60 | run: | 61 | cd ${{env.WORKSPACE_DIR }} 62 | catkin build -cs 63 | source install/setup.bash 64 | -------------------------------------------------------------------------------- /.github/workflows/clang-format.yaml: -------------------------------------------------------------------------------- 1 | name: clang-format 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | - devel 8 | - pull_req 9 | pull_request: 10 | branches: 11 | - master 12 | 13 | jobs: 14 | clang-format: 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - name: Checkout repository 19 | uses: actions/checkout@v3 20 | 21 | - name: Install Clang Format 22 | run: sudo apt-get update && sudo apt-get install -y clang-format 23 | 24 | - name: Check formatting 25 | working-directory: graph_core 26 | run: | 27 | FILES=$(find . -type f \( -name "*.cpp" -o -name "*.h" \)) 28 | 29 | CLANG_FORMAT_FILE=".clang-format" 30 | if [ ! -f "$CLANG_FORMAT_FILE" ]; then 31 | echo "Error: .clang-format file not found in the repository root." 32 | exit 1 33 | fi 34 | 35 | echo "Using .clang-format from: $(realpath "$CLANG_FORMAT_FILE")" 36 | 37 | for FILE in $FILES; do 38 | echo "Checking $FILE..." 39 | clang-format --style=file --assume-filename="$CLANG_FORMAT_FILE" -n $FILE #show where the issues are 40 | clang-format --Werror --style=file --assume-filename="$CLANG_FORMAT_FILE" -n $FILE #throw the error 41 | done 42 | 43 | echo "All files are correctly formatted." 44 | 45 | shell: bash 46 | 47 | -------------------------------------------------------------------------------- /.github/workflows/run_tests.yaml: -------------------------------------------------------------------------------- 1 | name: run-tests 2 | 3 | on: 4 | workflow_run: 5 | workflows: ["build"] # Name of the previous workflow 6 | types: 7 | - completed 8 | jobs: 9 | test: 10 | strategy: 11 | matrix: 12 | ubuntu_version: [ubuntu:20.04, ubuntu:22.04, ubuntu:latest] 13 | fail-fast: true 14 | runs-on: ubuntu-latest 15 | container: 16 | image: ${{ matrix.ubuntu_version }} 17 | 18 | steps: 19 | - name: Checkout repository 20 | uses: actions/checkout@v4 21 | 22 | - name: Install dependencies 23 | run: | 24 | export DEBIAN_FRONTEND=noninteractive 25 | apt update 26 | apt install -y cmake build-essential git libboost-all-dev libeigen3-dev libyaml-cpp-dev libpoco-dev liblog4cxx-dev libgtest-dev 27 | 28 | - name: Build & Install 29 | run: | 30 | export DEBIAN_FRONTEND=noninteractive 31 | mkdir -p build 32 | mkdir -p install 33 | 34 | export PATH_TO_WS="$(pwd)" 35 | echo "PATH_TO_WS=$PATH_TO_WS" >> "$GITHUB_ENV" 36 | 37 | echo "Workspace Path: $PATH_TO_WS" 38 | ls -la 39 | 40 | export PATH="$PATH_TO_WS/install/bin:$PATH" 41 | export LD_LIBRARY_PATH="$PATH_TO_WS/install/lib" 42 | export CMAKE_PREFIX_PATH="$PATH_TO_WS/install" 43 | 44 | echo "PATH=$PATH_TO_WS/install/bin:$PATH" >> "$GITHUB_ENV" 45 | echo "LD_LIBRARY_PATH=$PATH_TO_WS/install/lib" >> "$GITHUB_ENV" 46 | echo "CMAKE_PREFIX_PATH=$PATH_TO_WS/install" >> "$GITHUB_ENV" 47 | 48 | echo "PATH: $PATH" 49 | echo "LD_LIBRARY_PATH: $LD_LIBRARY_PATH" 50 | echo "CMAKE_PREFIX_PATH: $CMAKE_PREFIX_PATH" 51 | 52 | mkdir -p build/graph_core 53 | cmake -S graph_core -B build/graph_core -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install 54 | make -C build/graph_core install 55 | 56 | - name: Run GTESTS 57 | run: | 58 | export CNR_PARAM_ROOT_DIRECTORY="/tmp/cnr_param" 59 | cd ${{ env.PATH_TO_WS }}/install/bin/graph_core/tests/src 60 | ./run_all_tests logger_param.yaml 61 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.kdev4 3 | kdev4/ 4 | *.txt.user* 5 | *.orig* 6 | *.autosave* 7 | *.idea 8 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: local 3 | hooks: 4 | - id: clang-formatting-hook 5 | name: clang-formatting hook 6 | entry: ./hooks/pre-commit-clang-format 7 | language: script 8 | types: [file] 9 | files: \.(cpp|h)$ 10 | -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | ## Our Pledge 3 | We as members, contributors, and leaders pledge to make participation in our 4 | community a harassment-free experience for everyone, regardless of age, body 5 | size, visible or invisible disability, ethnicity, sex characteristics, gender 6 | identity and expression, level of experience, education, socio-economic status, 7 | nationality, personal appearance, race, religion, or sexual identity 8 | and orientation. 9 | 10 | We pledge to act and interact in ways that contribute to an open, welcoming, 11 | diverse, inclusive, and healthy community. 12 | 13 | ## Our Standards 14 | Examples of behavior that contributes to a positive environment for our 15 | community include: 16 | 17 | * Demonstrating empathy and kindness toward other people 18 | * Being respectful of differing opinions, viewpoints, and experiences 19 | * Giving and gracefully accepting constructive feedback 20 | * Accepting responsibility and apologizing to those affected by our mistakes, 21 | and learning from the experience 22 | * Focusing on what is best not just for us as individuals, but for the 23 | overall community 24 | 25 | Examples of unacceptable behavior include: 26 | 27 | * The use of sexualized language or imagery, and sexual attention or 28 | advances of any kind 29 | * Trolling, insulting or derogatory comments, and personal or political attacks 30 | * Public or private harassment 31 | * Publishing others' private information, such as a physical or email 32 | address, without their explicit permission 33 | * Other conduct which could reasonably be considered inappropriate in a 34 | professional setting 35 | 36 | ## Enforcement Responsibilities 37 | Community leaders are responsible for clarifying and enforcing our standards of 38 | acceptable behavior and will take appropriate and fair corrective action in 39 | response to any behavior that they deem inappropriate, threatening, offensive, 40 | or harmful. 41 | 42 | Community leaders have the right and responsibility to remove, edit, or reject 43 | comments, commits, code, wiki edits, issues, and other contributions that are 44 | not aligned to this Code of Conduct, and will communicate reasons for moderation 45 | decisions when appropriate. 46 | 47 | ## Scope 48 | This Code of Conduct applies within all community spaces, and also applies when 49 | an individual is officially representing the community in public spaces. 50 | Examples of representing our community include using an official e-mail address, 51 | posting via an official social media account, or acting as an appointed 52 | representative at an online or offline event. 53 | 54 | ## Enforcement 55 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 56 | reported to the community leaders responsible for enforcement at 57 | . 58 | All complaints will be reviewed and investigated promptly and fairly. 59 | 60 | All community leaders are obligated to respect the privacy and security of the 61 | reporter of any incident. 62 | 63 | ## Enforcement Guidelines 64 | Community leaders will follow these Community Impact Guidelines in determining 65 | the consequences for any action they deem in violation of this Code of Conduct: 66 | 67 | ### 1. Correction 68 | **Community Impact**: Use of inappropriate language or other behavior deemed 69 | unprofessional or unwelcome in the community. 70 | 71 | **Consequence**: A private, written warning from community leaders, providing 72 | clarity around the nature of the violation and an explanation of why the 73 | behavior was inappropriate. A public apology may be requested. 74 | 75 | ### 2. Warning 76 | **Community Impact**: A violation through a single incident or series 77 | of actions. 78 | 79 | **Consequence**: A warning with consequences for continued behavior. No 80 | interaction with the people involved, including unsolicited interaction with 81 | those enforcing the Code of Conduct, for a specified period of time. This 82 | includes avoiding interactions in community spaces as well as external channels 83 | like social media. Violating these terms may lead to a temporary or 84 | permanent ban. 85 | 86 | ### 3. Temporary Ban 87 | **Community Impact**: A serious violation of community standards, including 88 | sustained inappropriate behavior. 89 | 90 | **Consequence**: A temporary ban from any sort of interaction or public 91 | communication with the community for a specified period of time. No public or 92 | private interaction with the people involved, including unsolicited interaction 93 | with those enforcing the Code of Conduct, is allowed during this period. 94 | Violating these terms may lead to a permanent ban. 95 | 96 | ### 4. Permanent Ban 97 | **Community Impact**: Demonstrating a pattern of violation of community 98 | standards, including sustained inappropriate behavior, harassment of an 99 | individual, or aggression toward or disparagement of classes of individuals. 100 | 101 | **Consequence**: A permanent ban from any sort of public interaction within 102 | the community. 103 | 104 | ## Attribution 105 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], 106 | version 2.0, available at 107 | https://www.contributor-covenant.org/version/2/0/code_of_conduct.html. 108 | 109 | Community Impact Guidelines were inspired by [Mozilla's code of conduct 110 | enforcement ladder](https://github.com/mozilla/diversity). 111 | 112 | [homepage]: https://www.contributor-covenant.org 113 | 114 | For answers to common questions about this code of conduct, see the FAQ at 115 | https://www.contributor-covenant.org/faq. Translations are available at 116 | https://www.contributor-covenant.org/translations. 117 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2024 2 | 3 | JRL-CARI CNR-STIIMA/UNIBS 4 | Cesare Tonola, c.tonola001@unibs.it 5 | Manuel Beschi, manuel.beschi@unibs.it 6 | 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | 1. Redistributions of source code must retain the above copyright notice, 13 | this list of conditions and the following disclaimer. 14 | 15 | 2. Redistributions in binary form must reproduce the above copyright notice, 16 | this list of conditions and the following disclaimer in the documentation 17 | and/or other materials provided with the distribution. 18 | 19 | 3. Neither the name of the copyright holder nor the names of its 20 | contributors may be used to endorse or promote products derived from 21 | this software without specific prior written permission. 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 32 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | Graph Core Logo 3 |

4 | 5 | ## Introduction 6 | `graph_core` is an open-source C++ library for sampling-based robot path planning. It provides essential tools for solving path planning problems, includes state-of-the-art algorithms, and streamlines the development of new algorithms. 7 | 8 | ## Status 9 | [![build check](https://github.com/JRL-CARI-CNR-UNIBS/graph_core/actions/workflows/build_and_install.yaml/badge.svg)](https://github.com/JRL-CARI-CNR-UNIBS/graph_core/actions/workflows/build_and_install.yaml) 10 | [![clang-format check](https://github.com/JRL-CARI-CNR-UNIBS/graph_core/actions/workflows/clang-format.yaml/badge.svg)](https://github.com/JRL-CARI-CNR-UNIBS/graph_core/actions/workflows/clang-format.yaml) 11 | [![Codacy Badge](https://app.codacy.com/project/badge/Grade/1755d91be93e4c86912929a5e9ad04e8)](https://app.codacy.com/gh/JRL-CARI-CNR-UNIBS/graph_core/dashboard?utm_source=gh&utm_medium=referral&utm_content=&utm_campaign=Badge_grade) 12 | ![Status](https://img.shields.io/badge/License-BSD3-green) 13 | 14 | Developed and tested for Ubuntu 20.04, 22.04 and Ubuntu-latest. 15 | 16 | 17 | ## Tutorials 18 | See [this page](https://github.com/JRL-CARI-CNR-UNIBS/graph_core/blob/master/docs/tutorial/tutorial_intro.md) for tutorials. 19 | 20 | ## Dependencies 21 | `graph_core` depends on [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page), which can be installed with 22 | 23 | ```bash 24 | sudo apt update 25 | sudo apt -y install libeigen3-dev 26 | ``` 27 | 28 | Furthermore, it relies on the following packages: 29 | 30 | - [cnr_logger](https://github.com/CNR-STIIMA-IRAS/cnr_logger): Logging package. 31 | - [cnr_param](https://github.com/CNR-STIIMA-IRAS/cnr_param): Package to read and set parameters. It depends on [cnr_yaml](https://github.com/CNR-STIIMA-IRAS/cnr_yaml). 32 | - [cnr_class_loader](https://github.com/JRL-CARI-CNR-UNIBS/cnr_class_loader): Provides a way to load classes as plugins. 33 | 34 | These packages require the following system dependencies. Install them by running 35 | 36 | ```bash 37 | sudo apt update 38 | sudo apt -y install libboost-all-dev libyaml-cpp-dev libpoco-dev liblog4cxx-dev libgtest-dev 39 | ``` 40 | 41 | To simplify installation and dependency resolution, `graph_core` uses [CPM](https://github.com/cpm-cmake/CPM.cmake) to automatically download and integrate the required GitHub packages (`cnr_logger`, `cnr_yaml`, `cnr_param`, `cnr_class_loader`) into your build process. 42 | If you'd prefer to install the dependencies manually instead of relying on CPM, you can refer to the [cnr_common](https://github.com/JRL-CARI-CNR-UNIBS/cnr_common) page, or use [vcstool](https://github.com/dirk-thomas/vcstool) with the [`deps.repos` file](https://github.com/JRL-CARI-CNR-UNIBS/graph_core/deps.repos). In these cases, `graph_core` will automatically detect and link against the manually installed dependencies. Manual installation is recommended if other packages in your environment also depend on any of `graph_core`'s dependencies, ensuring consistency and avoiding redundant installations. 43 | 44 | ## Installation 45 | Follow these steps to compile and install `graph_core` using CMake. 46 | 47 | 1. Set the workspace directory path: 48 | ```bash 49 | export PATH_TO_WS=path_to_your_ws 50 | ``` 51 | 52 | 2. Compile and install `graph_core`: 53 | ```bash 54 | cd $PATH_TO_WS 55 | mkdir -p build/graph_core 56 | cmake -S src/graph_core/graph_core -B build/graph_core -DCMAKE_INSTALL_PREFIX=$PATH_TO_WS/install 57 | make -C build/graph_core install 58 | ``` 59 | 60 | ### Environment Configuration 61 | Add the following lines to your `~.bashrc` file: 62 | 63 | ```bash 64 | export PATH_TO_GRAPH_CORE_WS=path_to_your_ws #replace with the path to your workspace 65 | if [[ ":$PATH:" != *":${PATH_TO_GRAPH_CORE_WS}/install/bin:"* ]]; then 66 | export PATH="${PATH_TO_GRAPH_CORE_WS}/install/bin:$PATH" 67 | fi 68 | if [[ ":$CMAKE_PREFIX_PATH:" != *":${PATH_TO_GRAPH_CORE_WS}/install:"* ]]; then 69 | export CMAKE_PREFIX_PATH="${PATH_TO_GRAPH_CORE_WS}/install:$CMAKE_PREFIX_PATH" 70 | fi 71 | ``` 72 | 73 | ## Installing within a Catkin workspace 74 | To build `graph_core` within a Catkin workspace, ensure you have set `catkin config --install`. You do not need to export the paths as shown above, but you need to source the `install/setup.bash` file. 75 | 76 | In your `~/.bashrc`, add `source path_to_your_catkin_ws/install/setup.bash`. 77 | 78 | **Note**: If you installed `graph_core` dependencies automatically via CPM and another package in your workspace requires one of those dependencies (e.g., `cnr_param`) but not `graph_core`, you have two options: 79 | 80 | - Option 1 [Recommended]: Build and install `graph_core` (and its dependencies) in a non-catkin workspace, then build other packages in a secondary (cascade) catkin workspace. 81 | - Option 2: Download `graph_core` and its dependencies in a catkin workspace (e.g., using vcstool and the .repos file), build using `catkin build`, and source it. 82 | 83 | ## Final configuration 84 | The `cnr_param` library requires a directory to store its parameters. You can set this directory by adding the following line to your `~/.bashrc` file: 85 | 86 | ```bash 87 | export CNR_PARAM_ROOT_DIRECTORY="/tmp/cnr_param" 88 | ``` 89 | -------------------------------------------------------------------------------- /deps.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | cnr_yaml: 3 | type: git 4 | url: https://github.com/CNR-STIIMA-IRAS/cnr_yaml.git 5 | version: master 6 | cnr_param: 7 | type: git 8 | url: https://github.com/CNR-STIIMA-IRAS/cnr_param.git 9 | version: master 10 | cnr_logger: 11 | type: git 12 | url: https://github.com/CNR-STIIMA-IRAS/cnr_logger.git 13 | version: master 14 | cnr_class_loader: 15 | type: git 16 | url: https://github.com/JRL-CARI-CNR-UNIBS/cnr_class_loader.git 17 | version: modern_cmake -------------------------------------------------------------------------------- /docs/graph_core_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JRL-CARI-CNR-UNIBS/graph_core/b83dc26aa0c1a88f5db043cb099f223a397888b0/docs/graph_core_logo.png -------------------------------------------------------------------------------- /docs/graph_core_logo_background.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JRL-CARI-CNR-UNIBS/graph_core/b83dc26aa0c1a88f5db043cb099f223a397888b0/docs/graph_core_logo_background.png -------------------------------------------------------------------------------- /docs/graph_core_logo_blue.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JRL-CARI-CNR-UNIBS/graph_core/b83dc26aa0c1a88f5db043cb099f223a397888b0/docs/graph_core_logo_blue.png -------------------------------------------------------------------------------- /graph_core/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AlwaysBreakTemplateDeclarations: true 12 | AlwaysBreakBeforeMultilineStrings: true 13 | BreakBeforeBinaryOperators: false 14 | BreakBeforeTernaryOperators: false 15 | BreakConstructorInitializersBeforeComma: true 16 | BinPackParameters: true 17 | ColumnLimit: 120 18 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 19 | DerivePointerBinding: false 20 | PointerBindsToType: true 21 | ExperimentalAutoDetectBinPacking: false 22 | IndentCaseLabels: true 23 | MaxEmptyLinesToKeep: 1 24 | NamespaceIndentation: None 25 | ObjCSpaceBeforeProtocolList: true 26 | PenaltyBreakBeforeFirstCallParameter: 19 27 | PenaltyBreakComment: 60 28 | PenaltyBreakString: 1 29 | PenaltyBreakFirstLessLess: 1000 30 | PenaltyExcessCharacter: 1000 31 | PenaltyReturnTypeOnItsOwnLine: 90 32 | SpacesBeforeTrailingComments: 2 33 | Cpp11BracedListStyle: false 34 | Standard: Auto 35 | IndentWidth: 2 36 | TabWidth: 2 37 | UseTab: Never 38 | IndentFunctionDeclarationAfterType: false 39 | SpacesInParentheses: false 40 | SpacesInAngles: false 41 | SpaceInEmptyParentheses: false 42 | SpacesInCStyleCastParentheses: false 43 | SpaceAfterControlStatementKeyword: true 44 | SpaceBeforeAssignmentOperators: true 45 | ContinuationIndentWidth: 4 46 | SortIncludes: false 47 | SpaceAfterCStyleCast: false 48 | 49 | # Configure each individual brace in BraceWrapping 50 | BreakBeforeBraces: Custom 51 | 52 | # Control of individual brace wrapping cases 53 | BraceWrapping: 54 | AfterClass: true 55 | AfterControlStatement: true 56 | AfterEnum: true 57 | AfterFunction: true 58 | AfterNamespace: true 59 | AfterStruct: true 60 | AfterUnion: true 61 | BeforeCatch: true 62 | BeforeElse: true 63 | IndentBraces: false 64 | -------------------------------------------------------------------------------- /graph_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(graph_core VERSION 0.0.1) 3 | 4 | # Build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE "Release") 7 | #set(CMAKE_BUILD_TYPE "Debug") 8 | endif() 9 | 10 | # Compiler flags 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | if(CMAKE_BUILD_TYPE MATCHES "Release") 13 | add_compile_options(-funroll-loops -Wall -Ofast -flto -O3) 14 | endif() 15 | if(CMAKE_BUILD_TYPE MATCHES "Debug") 16 | add_compile_options(-Wall -g -Og) 17 | endif() 18 | endif() 19 | 20 | set(CMAKE_CXX_STANDARD 20) 21 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 22 | 23 | # Options 24 | option(ADD_CUSTOM_UNINSTALL "Add uninstall target" ON) 25 | option(BUILD_TESTS "Build the test executables" ON) 26 | 27 | # Dependencies 28 | include(cmake_config/Dependencies.cmake) 29 | 30 | # Configure RPATH for libraries 31 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 32 | list(APPEND CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 33 | set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 34 | set(CMAKE_SKIP_BUILD_RPATH FALSE) 35 | set(CMAKE_BUILD_RPATH "") 36 | 37 | message(STATUS "${BLUE}CMAKE_INSTALL_RPATH=${CMAKE_INSTALL_RPATH}${RESET}") 38 | 39 | # Library target 40 | add_library(${PROJECT_NAME} SHARED 41 | # Graph components 42 | src/${PROJECT_NAME}/graph/node.cpp 43 | src/${PROJECT_NAME}/graph/connection.cpp 44 | src/${PROJECT_NAME}/graph/tree.cpp 45 | src/${PROJECT_NAME}/graph/subtree.cpp 46 | src/${PROJECT_NAME}/graph/path.cpp 47 | src/${PROJECT_NAME}/graph/net.cpp 48 | 49 | # Samplers 50 | src/${PROJECT_NAME}/samplers/uniform_sampler.cpp 51 | src/${PROJECT_NAME}/samplers/ball_sampler.cpp 52 | src/${PROJECT_NAME}/samplers/informed_sampler.cpp 53 | src/${PROJECT_NAME}/samplers/tube_informed_sampler.cpp 54 | 55 | # Metrics 56 | src/${PROJECT_NAME}/metrics/euclidean_metrics.cpp 57 | 58 | # Datastructure 59 | src/${PROJECT_NAME}/datastructure/kdtree.cpp 60 | src/${PROJECT_NAME}/datastructure/vector.cpp 61 | 62 | # Solvers 63 | src/${PROJECT_NAME}/solvers/tree_solver.cpp 64 | src/${PROJECT_NAME}/solvers/rrt.cpp 65 | src/${PROJECT_NAME}/solvers/birrt.cpp 66 | src/${PROJECT_NAME}/solvers/rrt_star.cpp 67 | src/${PROJECT_NAME}/solvers/anytime_rrt.cpp 68 | 69 | # Path optimizers 70 | src/${PROJECT_NAME}/solvers/path_optimizers/path_optimizer_base.cpp 71 | src/${PROJECT_NAME}/solvers/path_optimizers/path_local_optimizer.cpp 72 | 73 | # Plugins 74 | src/${PROJECT_NAME}/plugins/metrics/euclidean_metrics_plugin.cpp 75 | src/${PROJECT_NAME}/plugins/metrics/null_goal_cost_function_plugin.cpp 76 | src/${PROJECT_NAME}/plugins/samplers/uniform_sampler_plugin.cpp 77 | src/${PROJECT_NAME}/plugins/samplers/ball_sampler_plugin.cpp 78 | src/${PROJECT_NAME}/plugins/samplers/informed_sampler_plugin.cpp 79 | src/${PROJECT_NAME}/plugins/solvers/rrt_plugin.cpp 80 | src/${PROJECT_NAME}/plugins/solvers/birrt_plugin.cpp 81 | src/${PROJECT_NAME}/plugins/solvers/anytime_rrt_plugin.cpp 82 | src/${PROJECT_NAME}/plugins/solvers/rrt_star_plugin.cpp 83 | ) 84 | 85 | # Add dependencies 86 | add_dependencies(${PROJECT_NAME} cnr_logger::cnr_logger cnr_param::cnr_param cnr_class_loader::cnr_class_loader) 87 | 88 | # Include directories 89 | target_include_directories(${PROJECT_NAME} PUBLIC 90 | $ 91 | $ 92 | ) 93 | 94 | # Link dependencies 95 | target_link_libraries(${PROJECT_NAME} PUBLIC 96 | Eigen3::Eigen 97 | cnr_logger::cnr_logger 98 | cnr_param::cnr_param 99 | cnr_class_loader::cnr_class_loader 100 | ) 101 | 102 | # Alias for library 103 | add_library("${PROJECT_NAME}::${PROJECT_NAME}" ALIAS ${PROJECT_NAME}) 104 | 105 | # Tests 106 | if(BUILD_TESTS) 107 | add_subdirectory(tests) 108 | endif() 109 | 110 | # Install headers 111 | install(DIRECTORY include/${PROJECT_NAME} 112 | DESTINATION include) 113 | 114 | # Install targets 115 | install( 116 | TARGETS ${PROJECT_NAME} 117 | EXPORT ${PROJECT_NAME}Targets 118 | ARCHIVE DESTINATION lib 119 | LIBRARY DESTINATION lib 120 | RUNTIME DESTINATION bin 121 | ) 122 | 123 | # Install exported targets 124 | install(EXPORT ${PROJECT_NAME}Targets 125 | DESTINATION "share/${PROJECT_NAME}/cmake" 126 | NAMESPACE ${PROJECT_NAME}:: 127 | FILE ${PROJECT_NAME}Targets.cmake 128 | ) 129 | 130 | # Package configuration 131 | include(CMakePackageConfigHelpers) 132 | include(GNUInstallDirs) 133 | 134 | write_basic_package_version_file( 135 | ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake 136 | VERSION ${PROJECT_VERSION} 137 | COMPATIBILITY AnyNewerVersion 138 | ) 139 | configure_package_config_file( 140 | ${CMAKE_SOURCE_DIR}/cmake_config/${PROJECT_NAME}Config.cmake.in 141 | ${PROJECT_NAME}Config.cmake 142 | INSTALL_DESTINATION share/${PROJECT_NAME}/cmake 143 | PATH_VARS CMAKE_INSTALL_INCLUDEDIR CMAKE_INSTALL_LIBDIR 144 | ) 145 | 146 | install(FILES 147 | ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake 148 | ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake 149 | DESTINATION "share/${PROJECT_NAME}/cmake" 150 | ) 151 | 152 | # Custom uninstall target 153 | if(ADD_CUSTOM_UNINSTALL) 154 | message(STATUS "Adding custom uninstall") 155 | add_custom_target("uninstall_graph_core" COMMENT "Uninstall installed files") 156 | add_custom_command( 157 | TARGET "uninstall_graph_core" 158 | POST_BUILD 159 | COMMAND xargs rm -vf < install_manifest.txt || echo Nothing to uninstall! 160 | ) 161 | endif() 162 | 163 | # Clang-format target 164 | file(GLOB_RECURSE ALL_SOURCE_FILES *.cpp *.h) 165 | 166 | add_custom_target( 167 | format 168 | COMMAND chmod +x cmake_config/clang_format_target.sh 169 | COMMAND cmake_config/clang_format_target.sh 170 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 171 | COMMENT "Running clang-format on all source files" 172 | ) 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /graph_core/cmake_config/Dependencies.cmake: -------------------------------------------------------------------------------- 1 | # CMake policies 2 | if(POLICY CMP0167) 3 | cmake_policy(SET CMP0167 NEW) 4 | cmake_policy(SET CMP0148 NEW) 5 | endif() 6 | 7 | # Deps 8 | find_package(Eigen3 REQUIRED COMPONENTS Core Dense Geometry) 9 | 10 | file( 11 | DOWNLOAD 12 | https://github.com/cpm-cmake/CPM.cmake/releases/download/v0.40.5/CPM.cmake 13 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake 14 | EXPECTED_HASH SHA256=c46b876ae3b9f994b4f05a4c15553e0485636862064f1fcc9d8b4f832086bc5d 15 | ) 16 | include(${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake) 17 | 18 | message("\n-----------------------------------------------\n") 19 | 20 | CPMFindPackage( 21 | NAME cnr_logger 22 | GITHUB_REPOSITORY CNR-STIIMA-IRAS/cnr_logger 23 | GIT_TAG master 24 | OPTIONS 25 | "USE_ROS1 OFF" 26 | "ENABLE_TESTING OFF" 27 | "ENABLE_COVERAGE_TESTING OFF" 28 | "COMPILE_EXAMPLE OFF" 29 | ) 30 | 31 | message("\n-----------------------------------------------\n") 32 | 33 | # If cnr_param is not already installed on the system, install cnr_yaml before cnr_param. 34 | # cnr_yaml could be already installed or missing. 35 | 36 | string(ASCII 27 Esc) 37 | set(RESET "${Esc}[m") 38 | set(BLUE "${Esc}[34m") 39 | 40 | find_package(cnr_param QUIET) 41 | 42 | if(NOT cnr_param_FOUND) 43 | # Check if cnr_yaml is already installed on the system 44 | find_package(cnr_yaml QUIET) 45 | 46 | if(NOT cnr_yaml_FOUND) 47 | # If cnr_yaml is not found, use CPM to install it 48 | CPMAddPackage( 49 | NAME cnr_yaml 50 | GITHUB_REPOSITORY CNR-STIIMA-IRAS/cnr_yaml 51 | GIT_TAG master 52 | OPTIONS 53 | "BUILD_UNIT_TESTS OFF" 54 | ) 55 | endif() 56 | endif() 57 | 58 | message("\n-----------------------------------------------\n") 59 | 60 | CPMFindPackage( 61 | NAME cnr_param 62 | GITHUB_REPOSITORY CNR-STIIMA-IRAS/cnr_param 63 | GIT_TAG master 64 | OPTIONS 65 | "COMPILE_MAPPED_FILE_MODULE ON" 66 | "BUILD_UNIT_TESTS OFF" 67 | "BUILD_INTEGRATION_TESTS OFF" 68 | "RETRIVE_DEPENDENCIES OFF" 69 | "BUILD_INTEGRATION_TESTS OFF" 70 | "BUILD_AS_A_CATKIN_PACKAGE OFF" 71 | ) 72 | 73 | message("\n-----------------------------------------------\n") 74 | 75 | CPMFindPackage( 76 | NAME cnr_class_loader 77 | GITHUB_REPOSITORY JRL-CARI-CNR-UNIBS/cnr_class_loader 78 | GIT_TAG modern_cmake 79 | ) 80 | 81 | message("\n-----------------------------------------------\n") 82 | -------------------------------------------------------------------------------- /graph_core/cmake_config/clang_format_target.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "Checking for clang-format..." 4 | 5 | if ! command -v clang-format > /dev/null 2>&1; then 6 | echo "clang-format not found, skipping format target" 7 | exit 0 8 | fi 9 | 10 | PROJECT_DIR="$(cd "$(dirname "$0")/.." && pwd)" 11 | echo "Searching for .clang-format file in $PROJECT_DIR..." 12 | 13 | CLANG_FORMAT_FILE=$(find "$PROJECT_DIR" -name ".clang-format" -print -quit) 14 | if [ -z "$CLANG_FORMAT_FILE" ]; then 15 | echo "No .clang-format file found in $PROJECT_DIR, skipping format target" 16 | exit 0 17 | fi 18 | 19 | echo "Found .clang-format file at $CLANG_FORMAT_FILE" 20 | 21 | echo "Running clang-format on the following files..." 22 | 23 | FILES=$(find "$PROJECT_DIR" -type f \( -name "*.cpp" -o -name "*.h" \)) 24 | 25 | if [ -z "$FILES" ]; then 26 | echo "No source files found in $PROJECT_DIR, skipping format target" 27 | exit 0 28 | fi 29 | 30 | echo "$FILES" 31 | 32 | for FILE in $FILES; do 33 | clang-format -i --style=file --assume-filename="$CLANG_FORMAT_FILE" "$FILE" 34 | done 35 | 36 | echo "clang-format completed successfully." 37 | -------------------------------------------------------------------------------- /graph_core/cmake_config/graph_coreConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include(CMakeFindDependencyMacro) 4 | 5 | find_dependency(Eigen3 REQUIRED COMPONENTS Core Dense Geometry) 6 | 7 | find_dependency(cnr_param REQUIRED) 8 | find_dependency(cnr_logger REQUIRED) 9 | find_dependency(cnr_class_loader REQUIRED) 10 | 11 | include("@CMAKE_INSTALL_PREFIX@/share/graph_core/cmake/graph_coreTargets.cmake") 12 | 13 | set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@" ${Eigen3_INCLUDE_DIRS} ${cnr_param_INCLUDE_DIRS} ${cnr_logger_INCLUDE_DIRS} ${cnr_class_loader_INCLUDE_DIRS}) 14 | set(@PROJECT_NAME@_LIBRARIES @PROJECT_NAME@ ${Eigen3_LIBRARIES} ${cnr_param_LIBRARIES} ${cnr_logger_LIBRARIES} ${cnr_class_loader_LIBRARIES}) 15 | 16 | 17 | check_required_components(@PROJECT_NAME@) 18 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/collision_checkers/cube_3d_collision_checker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "graph_core/collision_checkers/collision_checker_base.h" 4 | 5 | namespace graph 6 | { 7 | namespace core 8 | { 9 | /** 10 | * @class Cube3dCollisionChecker 11 | * @brief Collision checker for a 3D cube-shaped environment. 12 | * 13 | * The Cube3dCollisionChecker class inherits from CollisionCheckerBase and 14 | * implements a collision checker for a 3D environment represented as a cube. 15 | * The collision check is performed by verifying if the absolute values of the 16 | * configuration exceed a threshold, indicating a collision with the cube. 17 | */ 18 | 19 | class Cube3dCollisionChecker; 20 | typedef std::shared_ptr Cube3dCollisionCheckerPtr; 21 | 22 | class Cube3dCollisionChecker : public CollisionCheckerBase 23 | { 24 | public: 25 | /** 26 | * @brief If each joint has absolute value grater than 'abs_joint_threshold_', 27 | * the configuration will be considered collision-free. 28 | */ 29 | double abs_joint_threshold_; 30 | 31 | /** 32 | * @brief Constructor for Cube3dCollisionChecker. 33 | * @param logger Pointer to a TraceLogger for logging. 34 | * @param abs_joint_threshold is the value each joint must exceed in absolute 35 | * value for the configuration to be collision-free. 36 | * @param min_distance Distance between configurations checked for collisions 37 | * along a connection. 38 | */ 39 | Cube3dCollisionChecker(const cnr_logger::TraceLoggerPtr& logger, const double& abs_joint_threshold, 40 | const double& min_distance) 41 | : CollisionCheckerBase(logger, min_distance), abs_joint_threshold_(abs_joint_threshold) 42 | { 43 | } 44 | 45 | /** 46 | * @brief Constructor for Cube3dCollisionChecker. 47 | * @param logger Pointer to a TraceLogger for logging. 48 | * @param min_distance Distance between configurations checked for collisions 49 | * along a connection. 50 | */ 51 | Cube3dCollisionChecker(const cnr_logger::TraceLoggerPtr& logger, const double& min_distance = 0.01) 52 | : CollisionCheckerBase(logger, min_distance), abs_joint_threshold_(1.0) 53 | { 54 | } 55 | 56 | /** 57 | * @brief Check for collision with the 3D cube. 58 | * @param configuration The robot configuration to check for collision. 59 | * @return True if the configuration is in collision, false otherwise. 60 | */ 61 | virtual bool check(const Eigen::VectorXd& configuration) override 62 | { 63 | return configuration.cwiseAbs().maxCoeff() > abs_joint_threshold_; 64 | } 65 | 66 | /** 67 | * @brief Clone the collision checker. 68 | * @return A shared pointer to the cloned collision checker. 69 | */ 70 | virtual CollisionCheckerPtr clone() override 71 | { 72 | return std::make_shared(logger_, min_distance_); 73 | } 74 | }; 75 | 76 | } // end namespace core 77 | } // end namespace graph 78 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/datastructure/vector.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | PSEUDO CODE : 28 | - https://www.cs.cmu.edu/~ckingsf/bioinfo-lectures/kdtrees.pdf 29 | - http://andrewd.ces.clemson.edu/courses/cpsc805/references/nearest_search.pdf 30 | */ 31 | 32 | #include 33 | namespace graph 34 | { 35 | namespace core 36 | { 37 | /** 38 | * @class Vector 39 | * @brief NearestNeighbors implementation using a vector data structure for 40 | * storing nodes. 41 | */ 42 | class Vector : public NearestNeighbors 43 | { 44 | public: 45 | /** 46 | * @brief Constructor for the Vector class. 47 | */ 48 | Vector(const cnr_logger::TraceLoggerPtr& logger); 49 | 50 | /** 51 | * @brief Implementation of the insert function for adding a node to the 52 | * vector. 53 | * 54 | * @param node The node to be inserted. 55 | */ 56 | virtual void insert(const NodePtr& node) override; 57 | 58 | /** 59 | * @brief Implementation of the clear function to clear the nearest neighbors 60 | * data structure. Additionally, it sets size_ and delted_nodes_ to zero. 61 | * @return True if successful, false otherwise. 62 | */ 63 | virtual bool clear(); 64 | 65 | /** 66 | * @brief Implementation of the nearestNeighbor function for finding the 67 | * nearest neighbor in the vector. 68 | * 69 | * @param configuration The configuration for which the nearest neighbor needs 70 | * to be found. 71 | * @param best Reference to the pointer to the best-matching node. 72 | * @param best_distance Reference to the distance to the best-matching node. 73 | */ 74 | virtual void nearestNeighbor(const Eigen::VectorXd& configuration, NodePtr& best, double& best_distance) override; 75 | 76 | /** 77 | * @brief Implementation of the near function for finding nodes within a 78 | * specified radius in the vector. 79 | * 80 | * @param configuration The reference configuration. 81 | * @param radius The search radius. 82 | * @return A multimap containing nodes and their distances within the 83 | * specified radius. 84 | */ 85 | virtual std::multimap near(const Eigen::VectorXd& configuration, const double& radius) override; 86 | 87 | /** 88 | * @brief Implementation of the kNearestNeighbors function for finding k 89 | * nearest neighbors in the vector. 90 | * 91 | * @param configuration The reference configuration. 92 | * @param k The number of nearest neighbors to find. 93 | * @return A multimap containing k nodes and their distances. 94 | */ 95 | virtual std::multimap kNearestNeighbors(const Eigen::VectorXd& configuration, 96 | const size_t& k) override; 97 | 98 | /** 99 | * @brief Implementation of the findNode function for checking if a node 100 | * exists in the vector. 101 | * 102 | * @param node The node to check. 103 | * @return True if the node exists, false otherwise. 104 | */ 105 | virtual bool findNode(const NodePtr& node) override; 106 | 107 | /** 108 | * @brief Implementation of the deleteNode function for deleting a node from 109 | * the vector. 110 | * 111 | * @param node The node to delete. 112 | * @param disconnect_node If true, disconnect the node from the graph. 113 | * @return True if the deletion is successful, false otherwise. 114 | */ 115 | virtual bool deleteNode(const NodePtr& node, const bool& disconnect_node = false) override; 116 | 117 | /** 118 | * @brief Implementation of the restoreNode function for restoring a 119 | * previously deleted node (not implemented). 120 | * 121 | * @param node The node to restore. 122 | * @return Always returns false as restoration is not implemented. 123 | */ 124 | virtual bool restoreNode(const NodePtr& node) override; 125 | 126 | /** 127 | * @brief Implementation of the getNodes function for getting all nodes in the 128 | * vector. 129 | * 130 | * @return A vector containing all nodes in the vector. 131 | */ 132 | virtual std::vector getNodes() override; 133 | 134 | /** 135 | * @brief Implementation of the disconnectNodes function for disconnecting 136 | * nodes in the vector. 137 | * 138 | * @param white_list A vector of nodes to be excluded from the disconnection 139 | * process. 140 | */ 141 | virtual void disconnectNodes(const std::vector& white_list) override; 142 | 143 | protected: 144 | std::vector nodes_; 145 | }; 146 | 147 | } // end namespace core 148 | } // end namespace graph 149 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/metrics/euclidean_metrics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | /** 36 | * @class EuclideanMetrics 37 | * @brief This class defines a metrics which computes the cost between 38 | * configurations as their Euclidean distance. 39 | */ 40 | class EuclideanMetrics; 41 | typedef std::shared_ptr EuclideanMetricsPtr; 42 | 43 | // Euclidean metrics 44 | class EuclideanMetrics : public MetricsBase 45 | { 46 | protected: 47 | public: 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | 50 | /** 51 | * @brief Empty constructor for EuclideanMetrics. The function 52 | * MetricsBase::init() must be called afterwards. 53 | */ 54 | EuclideanMetrics() : MetricsBase() // set initialized_ false 55 | { 56 | } 57 | 58 | /** 59 | * @brief Constructs a EuclideanMetrics object. 60 | * @param logger A shared pointer to a TraceLogger for logging. 61 | */ 62 | EuclideanMetrics(const cnr_logger::TraceLoggerPtr& logger) : MetricsBase(logger) // set initialized_ true 63 | { 64 | } 65 | 66 | /** 67 | * @brief Calculates the cost between two configurations. 68 | * @param configuration1 The first node. 69 | * @param configuration2 The second node. 70 | * @return The cost between the two configurations. 71 | */ 72 | virtual double cost(const Eigen::VectorXd& configuration1, const Eigen::VectorXd& configuration2) override 73 | { 74 | return (configuration1 - configuration2).norm(); 75 | } 76 | virtual double cost(const NodePtr& node1, const NodePtr& node2) override 77 | { 78 | return cost(node1->getConfiguration(), node2->getConfiguration()); 79 | } 80 | 81 | /** 82 | * @brief Calculates the utopia (ideal minimum cost) between two 83 | * configurations. 84 | * @param configuration1 The first configuration. 85 | * @param configuration2 The second configuration. 86 | * @return The utopia distance between the two configurations. 87 | */ 88 | virtual double utopia(const Eigen::VectorXd& configuration1, const Eigen::VectorXd& configuration2) override 89 | { 90 | return (configuration1 - configuration2).norm(); 91 | } 92 | virtual double utopia(const NodePtr& node1, const NodePtr& node2) override 93 | { 94 | return utopia(node1->getConfiguration(), node2->getConfiguration()); 95 | } 96 | 97 | /** 98 | * @brief Creates a clone of the EuclideanMetrics object. 99 | * @return A shared pointer to the cloned EuclideanMetrics object. 100 | */ 101 | virtual MetricsPtr clone() override 102 | { 103 | return std::make_shared(logger_); 104 | } 105 | }; 106 | 107 | } // end namespace core 108 | } // end namespace graph 109 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/metrics/goal_cost_function_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | /** 36 | * @class GoalCostFunctionBase 37 | * @brief Base class for defining goal cost functions. 38 | * 39 | * The GoalCostFunctionBase class provides an interface for defining cost 40 | * functions associated with goal configurations in path planning. Users can 41 | * derive from this class to implement custom cost functions. 42 | */ 43 | class GoalCostFunctionBase 44 | { 45 | public: 46 | /** 47 | * @brief Default constructor for GoalCostFunctionBase. 48 | */ 49 | GoalCostFunctionBase() 50 | { 51 | } 52 | 53 | /** 54 | * @brief Calculate the cost for a given goal configuration. 55 | * @param q The goal configuration for which the cost is calculated. 56 | * @return The cost associated with the goal configuration. 57 | */ 58 | virtual double cost(const Eigen::VectorXd& q) 59 | { 60 | return 0; 61 | } 62 | double cost(const NodePtr& node) 63 | { 64 | return cost(node->getConfiguration()); 65 | } 66 | }; 67 | 68 | typedef std::shared_ptr GoalCostFunctionPtr; 69 | 70 | } // end namespace core 71 | } // end namespace graph 72 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/metrics/hamp_goal_cost_function_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class HampGoalCostFunctionBase 38 | * @brief Base class for defining goal cost functions. 39 | * 40 | * The HampGoalCostFunctionBase class provides an interface for defining cost 41 | * functions associated with goal configurations in HAMP path planning. Users 42 | * can derive from this class to implement custom cost functions. 43 | */ 44 | class HampGoalCostFunctionBase : public GoalCostFunctionBase 45 | { 46 | /** 47 | * @brief human_positions_ Matrix 3xn with the human positions. 48 | */ 49 | Eigen::Matrix human_positions_; 50 | 51 | /** 52 | * @brief human_velocities_ Matrix 3xn with the human velocities. 53 | */ 54 | Eigen::Matrix human_velocities_; 55 | 56 | public: 57 | /** 58 | * @brief Default constructor for GoalCostFunctionBase. 59 | */ 60 | HampGoalCostFunctionBase() 61 | { 62 | } 63 | 64 | /** 65 | * @brief Set human position. 66 | * 67 | * @param human_positions Matrix 3xn with the human positions 68 | */ 69 | virtual void setHumanPositions(const Eigen::Matrix& human_positions) 70 | { 71 | human_positions_ = human_positions; 72 | } 73 | 74 | /** 75 | * @brief Set human position. 76 | * 77 | * @param human_velocities Matrix 3xn with the human velocities 78 | */ 79 | virtual void setHumanVelocities(const Eigen::Matrix& human_velocities) 80 | { 81 | human_velocities_ = human_velocities; 82 | } 83 | }; 84 | 85 | typedef std::shared_ptr HampGoalCostFunctionPtr; 86 | 87 | } // end namespace core 88 | } // end namespace graph 89 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/metrics/hamp_metrics_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | /** 36 | * @class HampMetricsBase 37 | * @brief Base class for defining metrics to measure costs between 38 | * configurations usign human positions and velocities. 39 | * 40 | * The HampMetricsBase class provides an interface for cost evaluation 41 | * in path planning. Users can derive from this class to implement custom 42 | * metrics. 43 | */ 44 | class HampMetricsBase; 45 | typedef std::shared_ptr HampMetricsPtr; 46 | 47 | class HampMetricsBase : public MetricsBase 48 | { 49 | protected: 50 | /** 51 | * @brief human_positions_ Matrix 3xn with the human positions. 52 | */ 53 | Eigen::Matrix human_positions_; 54 | 55 | /** 56 | * @brief human_velocities_ Matrix 3xn with the human velocities. 57 | */ 58 | Eigen::Matrix human_velocities_; 59 | 60 | public: 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 62 | 63 | /** 64 | * @brief Empty constructor for MetricsBase. The function init() must be 65 | * called afterwards. 66 | */ 67 | HampMetricsBase() : MetricsBase() 68 | { 69 | } 70 | 71 | /** 72 | * @brief init Initialise the object, defining its main attributes. 73 | * @param logger Pointer to a TraceLogger for logging. 74 | * @return True if correctly initialised, False if already initialised. 75 | */ 76 | virtual bool init(const cnr_logger::TraceLoggerPtr& logger) 77 | { 78 | return MetricsBase::init(logger); 79 | } 80 | 81 | /** 82 | * @brief Set human position. 83 | * 84 | * @param human_positions Matrix 3xn with the human positions 85 | */ 86 | virtual void setHumanPositions(const Eigen::Matrix& human_positions) 87 | { 88 | human_positions_ = human_positions; 89 | } 90 | 91 | /** 92 | * @brief Set human position. 93 | * 94 | * @param human_velocities Matrix 3xn with the human velocities 95 | */ 96 | virtual void setHumanVelocities(const Eigen::Matrix& human_velocities) 97 | { 98 | human_velocities_ = human_velocities; 99 | } 100 | }; 101 | 102 | } // end namespace core 103 | } // end namespace graph 104 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/metrics/metrics_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | /** 36 | * @class MetricsBase 37 | * @brief Base class for defining metrics to measure costs between 38 | * configurations. 39 | * 40 | * The MetricsBase class provides an interface for cost evaluation 41 | * in path planning. Users can derive from this class to implement custom 42 | * metrics. 43 | */ 44 | class MetricsBase; 45 | typedef std::shared_ptr MetricsPtr; 46 | 47 | class MetricsBase : public std::enable_shared_from_this 48 | { 49 | protected: 50 | /** 51 | * @brief initialized_ Flag to indicate whether the object is initialised, 52 | * i.e. whether its members have been defined correctly. It is false when the 53 | * object is created with an empty constructor. In this case, call the 'init' 54 | * function to initialise it. The other constructors automatically initialise 55 | * the object. As long as the object is not initialised, it cannot perform its 56 | * main functions. 57 | */ 58 | bool initialized_; 59 | 60 | /** 61 | * @brief Pointer to a TraceLogger instance for logging. 62 | * 63 | * This member variable represents a pointer to a TraceLogger instance, 64 | * allowing to perform logging operations. TraceLogger is a part of the 65 | * cnr_logger library. Ensure that the logger is properly configured and 66 | * available for use. 67 | */ 68 | cnr_logger::TraceLoggerPtr logger_; 69 | 70 | public: 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 72 | 73 | /** 74 | * @brief Empty constructor for MetricsBase. The function init() must be 75 | * called afterwards. 76 | */ 77 | MetricsBase() 78 | { 79 | initialized_ = false; 80 | } 81 | 82 | /** 83 | * @brief Constructs a MetricsBase object. 84 | * @param logger A shared pointer to a TraceLogger for logging. 85 | */ 86 | MetricsBase(const cnr_logger::TraceLoggerPtr& logger) : logger_(logger) 87 | { 88 | initialized_ = true; 89 | } 90 | 91 | /** 92 | * @brief init Initialise the object, defining its main attributes. At the end 93 | * of the function, the flag 'initialized_' is set to true and the object can 94 | * execute its main functions. 95 | * @param logger Pointer to a TraceLogger for logging. 96 | * @return True if correctly initialised, False if already initialised. 97 | */ 98 | virtual bool init(const cnr_logger::TraceLoggerPtr& logger) 99 | { 100 | if (initialized_) 101 | { 102 | CNR_WARN(logger_, "Metrics already initialised!"); 103 | return false; 104 | } 105 | 106 | logger_ = logger; 107 | initialized_ = true; 108 | 109 | return true; 110 | } 111 | 112 | /** 113 | * @brief getInitialized tells if the object has been initialised. 114 | * @return the 'initialized_' flag. 115 | */ 116 | bool getInitialized() 117 | { 118 | return initialized_; 119 | } 120 | 121 | /** 122 | * @brief Calculates the cost between two configurations. 123 | * @param configuration1 The first node. 124 | * @param configuration2 The second node. 125 | * @return The cost between the two configurations. 126 | */ 127 | virtual double cost(const Eigen::VectorXd& configuration1, const Eigen::VectorXd& configuration2) = 0; 128 | virtual double cost(const NodePtr& node1, const NodePtr& node2) = 0; 129 | 130 | /** 131 | * @brief Calculates the utopia (ideal minimum cost) between two 132 | * configurations. 133 | * @param configuration1 The first configuration. 134 | * @param configuration2 The second configuration. 135 | * @return The utopia distance between the two configurations. 136 | */ 137 | virtual double utopia(const Eigen::VectorXd& configuration1, const Eigen::VectorXd& configuration2) = 0; 138 | virtual double utopia(const NodePtr& node1, const NodePtr& node2) = 0; 139 | /** 140 | * @brief Creates a clone of the MetricsBase object. 141 | * @return A shared pointer to the cloned Metrics object. 142 | */ 143 | virtual MetricsPtr clone() = 0; 144 | }; 145 | 146 | } // end namespace core 147 | } // end namespace graph 148 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/metrics/null_goal_cost_function.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | /** 36 | * @class NullGoalCostFunction 37 | * @brief class for setting goal cost to zero. 38 | * 39 | * The NullGoalCostFunction class is a placeholder to avoid the use of a cost 40 | * function 41 | */ 42 | class NullGoalCostFunction : public GoalCostFunctionBase 43 | { 44 | public: 45 | /** 46 | * @brief Default constructor for NullGoalCostFunction. 47 | */ 48 | NullGoalCostFunction(const cnr_logger::TraceLoggerPtr& logger) : logger_(logger) 49 | { 50 | } 51 | 52 | /** 53 | * @brief Calculate the cost for a given goal configuration. 54 | * @param q The goal configuration for which the cost is calculated. 55 | * @return The cost associated with the goal configuration. 56 | */ 57 | virtual double cost(const Eigen::VectorXd& q) 58 | { 59 | return 0.0; 60 | } 61 | 62 | protected: 63 | /** 64 | * @brief Pointer to a TraceLogger instance for logging. 65 | * 66 | * This member variable represents a pointer to a TraceLogger instance, 67 | * allowing to perform logging operations. TraceLogger is a part of the 68 | * cnr_logger library. Ensure that the logger is properly configured and 69 | * available for use. 70 | */ 71 | cnr_logger::TraceLoggerPtr logger_; 72 | }; 73 | 74 | } // end namespace core 75 | } // end namespace graph 76 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/collision_checkers/collision_checker_base_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class CollisionCheckerBasePlugin 38 | * @brief This class implements a wrapper to graph::core::CollisionCheckerBase 39 | * to allow its plugin to be defined. The class can be loaded as a plugin and 40 | * builds a graph::core::CollisionCheckerBase object. 41 | */ 42 | class CollisionCheckerBasePlugin; 43 | typedef std::shared_ptr CollisionCheckerPluginPtr; 44 | 45 | class CollisionCheckerBasePlugin : std::enable_shared_from_this 46 | { 47 | protected: 48 | /** 49 | * @brief collision_checker_ is the graph::core::CollisionCheckerBase object 50 | * built and initialized by this plugin class. 51 | */ 52 | graph::core::CollisionCheckerPtr collision_checker_; 53 | 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | /** 58 | * @brief Empty constructor for CollisionCheckerBase. The function init() must 59 | * be called afterwards. 60 | */ 61 | CollisionCheckerBasePlugin() 62 | { 63 | collision_checker_ = nullptr; 64 | } 65 | 66 | /** 67 | * @brief Destructor for CollisionCheckerBasePlugin. 68 | */ 69 | virtual ~CollisionCheckerBasePlugin() 70 | { 71 | collision_checker_ = nullptr; 72 | } 73 | 74 | /** 75 | * @brief getCollisionChecker return the graph::core::CollisionCheckerPtr 76 | * object built by the plugin. 77 | * @return the graph::core::CollisionCheckerPtr object built. 78 | */ 79 | graph::core::CollisionCheckerPtr getCollisionChecker() 80 | { 81 | return collision_checker_; 82 | } 83 | 84 | /** 85 | * @brief init Initialise the graph::core::CollisionCheckerBase object, 86 | * defining its main attributes. 87 | * @param param_ns defines the namespace under which parameter are searched 88 | * for using cnr_param library. MoveitCollisionChecker requires group_name and 89 | * checker_resolution as parameters. 90 | * @param planning_scene Pointer to the MoveIt! PlanningScene. 91 | * @param logger Pointer to a TraceLogger for logging. 92 | * @return True if correctly initialised, False if already initialised. 93 | */ 94 | virtual bool init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger, 95 | const double& min_distance = 0.01) = 0; 96 | }; 97 | 98 | } // namespace core 99 | } // namespace graph 100 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/metrics/euclidean_metrics_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace graph 34 | { 35 | namespace core 36 | { 37 | /** 38 | * @class EuclideanMetricsPlugin 39 | * @brief This class implements a wrapper to graph::core::EuclideanMetrics to 40 | * allow its plugin to be defined. 41 | */ 42 | class EuclideanMetricsPlugin : public MetricsBasePlugin 43 | { 44 | protected: 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | /** 49 | * @brief Empty constructor for EuclideanMetricsPlugin. The function 50 | * EuclideanMetricsPlugin::init() must be called afterwards. 51 | */ 52 | EuclideanMetricsPlugin() : MetricsBasePlugin() 53 | { 54 | } 55 | 56 | /** 57 | * @brief init Initialise the object, defining its main attributes. At the end 58 | * of the function, the flag 'init_' is set to true and the object can execute 59 | * its main functions. 60 | * @param param_ns defines the namespace under which parameter are searched 61 | * for using cnr_param library. 62 | * @param logger Pointer to a TraceLogger for logging. 63 | * @return True if correctly initialised, False if already initialised. 64 | */ 65 | virtual bool init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger) override 66 | { 67 | metrics_ = std::make_shared(logger); 68 | return true; 69 | } 70 | }; 71 | 72 | } // namespace core 73 | } // namespace graph 74 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/metrics/goal_cost_function_base_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class GoalCostFunctionBasePlugin 38 | * @brief This class implements a wrapper to graph::core::GoalCostFunctionBase 39 | * to allow its plugin to be defined. The class can be loaded as a plugin and 40 | * builds a graph::core::GoalCostFunctionBase object. 41 | */ 42 | class GoalCostFunctionBasePlugin; 43 | typedef std::shared_ptr GoalCostFunctionPluginPtr; 44 | 45 | class GoalCostFunctionBasePlugin : public std::enable_shared_from_this 46 | { 47 | protected: 48 | /** 49 | * @brief metrics_ is the graph::core::GoalCostFunctionBase object built and 50 | * initialized by this plugin class. 51 | */ 52 | graph::core::GoalCostFunctionPtr goal_cost_fcn_; 53 | 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | /** 58 | * @brief Empty constructor for GoalCostFunctionBasePlugin. The function 59 | * init() must be called afterwards. 60 | */ 61 | GoalCostFunctionBasePlugin() 62 | { 63 | goal_cost_fcn_ = nullptr; 64 | } 65 | 66 | /** 67 | * @brief Destructor for GoalCostFunctionBasePlugin. 68 | */ 69 | virtual ~GoalCostFunctionBasePlugin() 70 | { 71 | goal_cost_fcn_ = nullptr; 72 | } 73 | 74 | /** 75 | * @brief getCostFunction return the graph::core::GoalCostFunctionPtr object 76 | * built by the plugin. 77 | * @return the graph::core::GoalCostFunctionPtr object built. 78 | */ 79 | virtual graph::core::GoalCostFunctionPtr getCostFunction() 80 | { 81 | return goal_cost_fcn_; 82 | } 83 | 84 | /** 85 | * @brief init Initialise the graph::core::GoalCostFunctionBase object, 86 | * defining its main attributes. 87 | * @param param_ns defines the namespace under which parameter are searched 88 | * for using cnr_param library. 89 | * @param logger Pointer to a TraceLogger for logging. 90 | * @return True if correctly initialised, False if already initialised. 91 | */ 92 | virtual bool init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger) = 0; 93 | }; 94 | 95 | } // namespace core 96 | } // namespace graph 97 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/metrics/hamp_goal_cost_function_base_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, UNIBS and CNR-STIIMA, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class GoalCostFunctionBasePlugin 38 | * @brief This class implements a wrapper to graph::core::GoalCostFunctionBase 39 | * to allow its plugin to be defined. The class can be loaded as a plugin and 40 | * builds a graph::core::GoalCostFunctionBase object. 41 | */ 42 | class HampGoalCostFunctionBasePlugin; 43 | typedef std::shared_ptr HampGoalCostFunctionPluginPtr; 44 | 45 | class HampGoalCostFunctionBasePlugin : public std::enable_shared_from_this 46 | { 47 | protected: 48 | /** 49 | * @brief metrics_ is the graph::core::GoalCostFunctionBase object built and 50 | * initialized by this plugin class. 51 | */ 52 | graph::core::HampGoalCostFunctionPtr goal_cost_fcn_; 53 | 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | /** 58 | * @brief Empty constructor for HampGoalCostFunctionBasePlugin. The function 59 | * init() must be called afterwards. 60 | */ 61 | HampGoalCostFunctionBasePlugin() 62 | { 63 | goal_cost_fcn_ = nullptr; 64 | } 65 | 66 | /** 67 | * @brief Destructor for GoalCostFunctionBasePlugin. 68 | */ 69 | virtual ~HampGoalCostFunctionBasePlugin() 70 | { 71 | goal_cost_fcn_ = nullptr; 72 | } 73 | 74 | /** 75 | * @brief getCostFunction return the graph::core::GoalCostFunctionPtr object 76 | * built by the plugin. 77 | * @return the graph::core::GoalCostFunctionPtr object built. 78 | */ 79 | virtual graph::core::HampGoalCostFunctionPtr getCostFunction() 80 | { 81 | return goal_cost_fcn_; 82 | } 83 | 84 | /** 85 | * @brief init Initialise the graph::core::GoalCostFunctionBase object, 86 | * defining its main attributes. 87 | * @param param_ns defines the namespace under which parameter are searched 88 | * for using cnr_param library. 89 | * @param logger Pointer to a TraceLogger for logging. 90 | * @return True if correctly initialised, False if already initialised. 91 | */ 92 | virtual bool init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger) = 0; 93 | }; 94 | 95 | } // namespace core 96 | } // namespace graph 97 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/metrics/hamp_metrics_base_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, UNIBS and CNR-STIIMA, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class HampMetricsBase 38 | * @brief This class implements a wrapper to graph::core::HampMetricsBase to 39 | * allow its plugin to be defined. The class can be loaded as a plugin and 40 | * builds a graph::core::HampMetricsBase object. 41 | */ 42 | class HampMetricsBasePlugin : public std::enable_shared_from_this 43 | { 44 | protected: 45 | /** 46 | * @brief metrics_ is the graph::core::HampMetricsBase object built and 47 | * initialized by this plugin class. 48 | */ 49 | graph::core::HampMetricsPtr metrics_; 50 | 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | 54 | /** 55 | * @brief Empty constructor for HampMetricsBasePlugin. The function init() 56 | * must be called afterwards. 57 | */ 58 | HampMetricsBasePlugin() 59 | { 60 | metrics_ = nullptr; 61 | } 62 | 63 | /** 64 | * @brief Destructor for HampMetricsBasePlugin. 65 | */ 66 | virtual ~HampMetricsBasePlugin() 67 | { 68 | metrics_ = nullptr; 69 | } 70 | 71 | /** 72 | * @brief getMetrics return the graph::core::HampMetricsPtr object built by 73 | * the plugin. 74 | * @return the graph::core::HampMetricsPtr object built. 75 | */ 76 | virtual graph::core::HampMetricsPtr getMetrics() 77 | { 78 | return metrics_; 79 | } 80 | 81 | /** 82 | * @brief init Initialise the graph::core::HampMetricsBase object, defining 83 | * its main attributes. 84 | * @param param_ns defines the namespace under which parameter are searched 85 | * for using cnr_param library. 86 | * @param logger Pointer to a TraceLogger for logging. 87 | * @return True if correctly initialised, False if already initialised. 88 | */ 89 | virtual bool init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger) = 0; 90 | }; 91 | 92 | } // namespace core 93 | } // namespace graph 94 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/metrics/metrics_base_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class MetricsBasePlugin 38 | * @brief This class implements a wrapper to graph::core::MetricsBase to allow 39 | * its plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::MetricsBase object. 41 | */ 42 | class MetricsBasePlugin; 43 | typedef std::shared_ptr MetricsPluginPtr; 44 | 45 | class MetricsBasePlugin : public std::enable_shared_from_this 46 | { 47 | protected: 48 | /** 49 | * @brief metrics_ is the graph::core::MetricsBase object built and 50 | * initialized by this plugin class. 51 | */ 52 | graph::core::MetricsPtr metrics_; 53 | 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | /** 58 | * @brief Empty constructor for MetricsBasePlugin. The function init() must be 59 | * called afterwards. 60 | */ 61 | MetricsBasePlugin() 62 | { 63 | metrics_ = nullptr; 64 | } 65 | 66 | /** 67 | * @brief Destructor for MetricsBasePlugin. 68 | */ 69 | virtual ~MetricsBasePlugin() 70 | { 71 | metrics_ = nullptr; 72 | } 73 | 74 | /** 75 | * @brief getMetrics return the graph::core::MetricsPtr object built by the 76 | * plugin. 77 | * @return the graph::core::MetricsPtr object built. 78 | */ 79 | virtual graph::core::MetricsPtr getMetrics() 80 | { 81 | return metrics_; 82 | } 83 | 84 | /** 85 | * @brief init Initialise the graph::core::MetricsBase object, defining its 86 | * main attributes. 87 | * @param param_ns defines the namespace under which parameter are searched 88 | * for using cnr_param library. 89 | * @param logger Pointer to a TraceLogger for logging. 90 | * @return True if correctly initialised, False if already initialised. 91 | */ 92 | virtual bool init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger) = 0; 93 | }; 94 | 95 | } // namespace core 96 | } // namespace graph 97 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/metrics/null_goal_cost_function_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace graph 34 | { 35 | namespace core 36 | { 37 | /** 38 | * @class NullGoalCostFunctionPlugin 39 | * @brief This class implements a wrapper to graph::core::NullGoalCostFunction 40 | * to allow its plugin to be defined. The class can be loaded as a plugin and 41 | * builds a graph::core::NullGoalCostFunction object. 42 | */ 43 | class NullGoalCostFunctionPlugin : public GoalCostFunctionBasePlugin 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | /** 49 | * @brief Empty constructor for NullGoalCostFunctionPlugin. The function 50 | * init() must be called afterwards. 51 | */ 52 | NullGoalCostFunctionPlugin() 53 | { 54 | goal_cost_fcn_ = nullptr; 55 | } 56 | 57 | /** 58 | * @brief Destructor for NullGoalCostFunctionPlugin. 59 | */ 60 | virtual ~NullGoalCostFunctionPlugin() 61 | { 62 | goal_cost_fcn_ = nullptr; 63 | } 64 | 65 | /** 66 | * @brief init Initialise the graph::core::NullGoalCostFunction object, 67 | * defining its main attributes. 68 | * @param param_ns defines the namespace under which parameter are searched 69 | * for using cnr_param library. 70 | * @param logger Pointer to a TraceLogger for logging. 71 | * @return True if correctly initialised, False if already initialised. 72 | */ 73 | virtual bool init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger); 74 | }; 75 | 76 | } // namespace core 77 | } // namespace graph 78 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/samplers/ball_sampler_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class BallSamplerPlugin 38 | * @brief This class implements a wrapper to graph::core::BallSampler to allow 39 | * its plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::BallSampler object. 41 | */ 42 | class BallSamplerPlugin : public SamplerBasePlugin 43 | { 44 | protected: 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | /** 49 | * @brief Empty constructor for BallSamplerPlugin. The function init() must be 50 | * called afterwards. 51 | */ 52 | BallSamplerPlugin() : SamplerBasePlugin() 53 | { 54 | } 55 | 56 | /** 57 | * @brief init Initialise the object graph::core::BallSampler, defining its 58 | * main attributes. 59 | * @param param_ns defines the namespace under which parameter are searched 60 | * for using cnr_param library. 61 | * @param focus_1 is considered as the center of the ball. 62 | * @param focus_2 discarded, it just has no effects. 63 | * @param lower_bound Lower bounds for each dimension. 64 | * @param upper_bound Upper bounds for each dimension. 65 | * @param scale Scaling factors for each dimension, no effects for this class. 66 | * @param logger TraceLogger for logging. 67 | * @param cost Radius of the ball (default: infinity). 68 | * @return True if correctly initialised, False if already initialised. 69 | */ 70 | virtual bool init(const std::string& param_ns, const Eigen::VectorXd& focus_1, const Eigen::VectorXd& focus_2, 71 | const Eigen::VectorXd& lower_bound, const Eigen::VectorXd& upper_bound, 72 | const Eigen::VectorXd& scale, const cnr_logger::TraceLoggerPtr& logger, 73 | const double& cost = std::numeric_limits::infinity()) override 74 | { 75 | (void)param_ns; 76 | (void)focus_2; 77 | (void)scale; 78 | sampler_ = std::make_shared(focus_1, lower_bound, upper_bound, logger, cost); 79 | return true; 80 | } 81 | }; 82 | 83 | } // namespace core 84 | } // namespace graph 85 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/samplers/informed_sampler_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class InformedSamplerPlugin 38 | * @brief This class implements a wrapper to graph::core::InformedSampler to 39 | * allow its plugin to be defined. The class can be loaded as a plugin and 40 | * builds a graph::core::InformedSampler object. 41 | */ 42 | class InformedSamplerPlugin : public SamplerBasePlugin 43 | { 44 | protected: 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | /** 49 | * @brief Empty constructor for InformedSamplerPlugin. The function init() 50 | * must be called afterwards. 51 | */ 52 | InformedSamplerPlugin() : SamplerBasePlugin() 53 | { 54 | } 55 | 56 | /** 57 | * @brief init Initialise the object graph::core::InformedSampler, defining 58 | * its main attributes. 59 | * @param param_ns defines the namespace under which parameter are searched 60 | * for using cnr_param library. 61 | * @param focus_1 focus 1 for the ellipse. 62 | * @param focus_2 focus 2 for the ellipse. 63 | * @param lower_bound Lower bounds for each dimension. 64 | * @param upper_bound Upper bounds for each dimension. 65 | * @param scale Scaling factors for each dimension. 66 | * @param logger TraceLogger for logging. 67 | * @param cost Cost of the path (default: infinity). 68 | * @return True if correctly initialised, False if already initialised. 69 | */ 70 | virtual bool init(const std::string& param_ns, const Eigen::VectorXd& focus_1, const Eigen::VectorXd& focus_2, 71 | const Eigen::VectorXd& lower_bound, const Eigen::VectorXd& upper_bound, 72 | const Eigen::VectorXd& scale, const cnr_logger::TraceLoggerPtr& logger, 73 | const double& cost = std::numeric_limits::infinity()) override 74 | { 75 | sampler_ = std::make_shared(focus_1, focus_2, lower_bound, upper_bound, scale, logger, cost); 76 | return true; 77 | } 78 | }; 79 | 80 | } // namespace core 81 | } // namespace graph 82 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/samplers/sampler_base_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class SamplerBasePlugin 38 | * @brief This class implements a wrapper to graph::core::SamplerBase to allow 39 | * its plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::SamplerBase object. 41 | */ 42 | class SamplerBasePlugin; 43 | typedef std::shared_ptr SamplerPluginPtr; 44 | 45 | class SamplerBasePlugin : public std::enable_shared_from_this 46 | { 47 | protected: 48 | /** 49 | * @brief sampler_ is the graph::core::SamplerBase object built and 50 | * initialized by this plugin class. 51 | */ 52 | graph::core::SamplerPtr sampler_; 53 | 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | /** 58 | * @brief Empty constructor for SamplerBasePlugin. The function init() must be 59 | * called afterwards. 60 | */ 61 | SamplerBasePlugin() 62 | { 63 | sampler_ = nullptr; 64 | } 65 | 66 | /** 67 | * @brief Destructor for SamplerBasePlugin. 68 | */ 69 | virtual ~SamplerBasePlugin() 70 | { 71 | sampler_ = nullptr; 72 | } 73 | 74 | /** 75 | * @brief getSampler return the graph::core::SamplerPtr object built by the 76 | * plugin. 77 | * @return the graph::core::SamplerPtr object built. 78 | */ 79 | virtual graph::core::SamplerPtr getSampler() 80 | { 81 | return sampler_; 82 | } 83 | 84 | /** 85 | * @brief init Initialise the object graph::core::SamplerBase, defining its 86 | * main attributes. 87 | * @param param_ns defines the namespace under which parameter are searched 88 | * for using cnr_param library. 89 | * @param focus_1 focus 1 for the ellipse. 90 | * @param focus_2 focus 2 for the ellipse. 91 | * @param lower_bound Lower bounds for each dimension. 92 | * @param upper_bound Upper bounds for each dimension. 93 | * @param scale Scaling factors for each dimension. 94 | * @param logger TraceLogger for logging. 95 | * @param cost Cost of the path (default: infinity). 96 | * @return True if correctly initialised, False if already initialised. 97 | */ 98 | virtual bool init(const std::string& param_ns, const Eigen::VectorXd& focus_1, const Eigen::VectorXd& focus_2, 99 | const Eigen::VectorXd& lower_bound, const Eigen::VectorXd& upper_bound, 100 | const Eigen::VectorXd& scale, const cnr_logger::TraceLoggerPtr& logger, 101 | const double& cost = std::numeric_limits::infinity()) = 0; 102 | }; 103 | 104 | } // namespace core 105 | } // namespace graph 106 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/samplers/uniform_sampler_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class UniformSamplerPlugin 38 | * @brief This class implements a wrapper to graph::core::UniformSampler to 39 | * allow its plugin to be defined. The class can be loaded as a plugin and 40 | * builds a graph::core::UniformSampler object. 41 | */ 42 | class UniformSamplerPlugin : public SamplerBasePlugin 43 | { 44 | protected: 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | /** 49 | * @brief Empty constructor for UniformSamplerPlugin. The function init() must 50 | * be called afterwards. 51 | */ 52 | UniformSamplerPlugin() : SamplerBasePlugin() 53 | { 54 | } 55 | 56 | /** 57 | * @brief init Initialise the object graph::core::UniformSampler, defining its 58 | * main attributes. 59 | * @param param_ns defines the namespace under which parameter are searched 60 | * for using cnr_param library. 61 | * @param focus_1 discarded, it just has no effects. 62 | * @param focus_2 discarded, it just has no effects. 63 | * @param lower_bound Lower bounds for each dimension. 64 | * @param upper_bound Upper bounds for each dimension. 65 | * @param scale Scaling factors for each dimension, no effects for this class. 66 | * @param logger TraceLogger for logging. 67 | * @param cost discarded, it just has no effects. 68 | * @return True if correctly initialised, False if already initialised. 69 | */ 70 | virtual bool init(const std::string& param_ns, const Eigen::VectorXd& focus_1, const Eigen::VectorXd& focus_2, 71 | const Eigen::VectorXd& lower_bound, const Eigen::VectorXd& upper_bound, 72 | const Eigen::VectorXd& scale, const cnr_logger::TraceLoggerPtr& logger, 73 | const double& cost = std::numeric_limits::infinity()) override 74 | { 75 | (void)param_ns; 76 | (void)focus_2; 77 | (void)scale; 78 | sampler_ = std::make_shared(lower_bound, upper_bound, logger); 79 | return true; 80 | } 81 | }; 82 | 83 | } // namespace core 84 | } // namespace graph 85 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/solvers/anytime_rrt_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class AnytimeRRTPlugin 38 | * @brief This class implements a wrapper to graph::core::AnytimeRRT to allow 39 | * its plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::AnytimeRRT object. 41 | */ 42 | 43 | class AnytimeRRTPlugin : public TreeSolverPlugin 44 | { 45 | protected: 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | /** 50 | * @brief Empty constructor for AnytimeRRTPlugin. The function 51 | * AnytimeRRTPlugin::init() must be called afterwards. 52 | */ 53 | AnytimeRRTPlugin() : TreeSolverPlugin() 54 | { 55 | } 56 | 57 | /** 58 | * @brief init Initialise the object, defining its main attributes. 59 | * @param param_ns defines the namespace under which parameter are searched 60 | * for using cnr_param library. 61 | * @param metrics The metrics used to evaluate paths. 62 | * @param checker The collision checker for checking collisions. 63 | * @param sampler The sampler for generating random configurations. 64 | * @param goal_cost_fcn The function used to assign the cost of the goal. If 65 | * it is not defined, the default cost function does not assign any cost to 66 | * the goal. 67 | * @param logger The logger for logging messages. 68 | * @return True if correctly initialised, False if already initialised. 69 | */ 70 | virtual bool init(const std::string& param_ns, const graph::core::MetricsPtr& metrics, 71 | const graph::core::CollisionCheckerPtr& checker, const graph::core::SamplerPtr& sampler, 72 | const graph::core::GoalCostFunctionPtr& goal_cost_fcn, const cnr_logger::TraceLoggerPtr& logger) 73 | { 74 | solver_ = std::make_shared(metrics, checker, sampler, goal_cost_fcn, logger); 75 | return true; 76 | } 77 | }; 78 | 79 | } // namespace core 80 | } // namespace graph 81 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/solvers/birrt_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class BiRRTPlugin 38 | * @brief This class implements a wrapper to graph::core::BiRRT to allow its 39 | * plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::BiRRT object. 41 | */ 42 | 43 | class BiRRTPlugin : public TreeSolverPlugin 44 | { 45 | protected: 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | /** 50 | * @brief Empty constructor for BiRRTPlugin. The function BiRRTPlugin::init() 51 | * must be called afterwards. 52 | */ 53 | BiRRTPlugin() : TreeSolverPlugin() 54 | { 55 | } 56 | 57 | /** 58 | * @brief init Initialise the object, defining its main attributes. 59 | * @param param_ns defines the namespace under which parameter are searched 60 | * for using cnr_param library. 61 | * @param metrics The metrics used to evaluate paths. 62 | * @param checker The collision checker for checking collisions. 63 | * @param sampler The sampler for generating random configurations. 64 | * @param goal_cost_fcn The function used to assign the cost of the goal. If 65 | * it is not defined, the default cost function does not assign any cost to 66 | * the goal. 67 | * @param logger The logger for logging messages. 68 | * @return True if correctly initialised, False if already initialised. 69 | */ 70 | virtual bool init(const std::string& param_ns, const graph::core::MetricsPtr& metrics, 71 | const graph::core::CollisionCheckerPtr& checker, const graph::core::SamplerPtr& sampler, 72 | const graph::core::GoalCostFunctionPtr& goal_cost_fcn, const cnr_logger::TraceLoggerPtr& logger) 73 | { 74 | solver_ = std::make_shared(metrics, checker, sampler, goal_cost_fcn, logger); 75 | return true; 76 | } 77 | }; 78 | 79 | } // namespace core 80 | } // namespace graph 81 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/solvers/rrt_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class RRTPlugin 38 | * @brief This class implements a wrapper to graph::core::RRT to allow its 39 | * plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::RRT object. 41 | */ 42 | 43 | class RRTPlugin : public TreeSolverPlugin 44 | { 45 | protected: 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | /** 50 | * @brief Empty constructor for RRTPlugin. The function RRTPlugin::init() must 51 | * be called afterwards. 52 | */ 53 | RRTPlugin() : TreeSolverPlugin() 54 | { 55 | } 56 | 57 | /** 58 | * @brief init Initialise the object, defining its main attributes. 59 | * @param param_ns defines the namespace under which parameter are searched 60 | * for using cnr_param library. 61 | * @param metrics The metrics used to evaluate paths. 62 | * @param checker The collision checker for checking collisions. 63 | * @param sampler The sampler for generating random configurations. 64 | * @param goal_cost_fcn The function used to assign the cost of the goal. If 65 | * it is not defined, the default cost function does not assign any cost to 66 | * the goal. 67 | * @param logger The logger for logging messages. 68 | * @return True if correctly initialised, False if already initialised. 69 | */ 70 | virtual bool init(const std::string& param_ns, const graph::core::MetricsPtr& metrics, 71 | const graph::core::CollisionCheckerPtr& checker, const graph::core::SamplerPtr& sampler, 72 | const graph::core::GoalCostFunctionPtr& goal_cost_fcn, const cnr_logger::TraceLoggerPtr& logger) 73 | { 74 | solver_ = std::make_shared(metrics, checker, sampler, goal_cost_fcn, logger); 75 | return true; 76 | } 77 | }; 78 | 79 | } // namespace core 80 | } // namespace graph 81 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/solvers/rrt_star_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class RRTStarPlugin 38 | * @brief This class implements a wrapper to graph::core::RRTStar to allow its 39 | * plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::RRTStar object. 41 | */ 42 | 43 | class RRTStarPlugin : public TreeSolverPlugin 44 | { 45 | protected: 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | /** 50 | * @brief Empty constructor for RRTStarPlugin. The function 51 | * RRTStarPlugin::init() must be called afterwards. 52 | */ 53 | RRTStarPlugin() : TreeSolverPlugin() 54 | { 55 | } 56 | 57 | /** 58 | * @brief init Initialise the object, defining its main attributes. 59 | * @param param_ns defines the namespace under which parameter are searched 60 | * for using cnr_param library. 61 | * @param metrics The metrics used to evaluate paths. 62 | * @param checker The collision checker for checking collisions. 63 | * @param sampler The sampler for generating random configurations. 64 | * @param goal_cost_fcn The function used to assign the cost of the goal. If 65 | * it is not defined, the default cost function does not assign any cost to 66 | * the goal. 67 | * @param logger The logger for logging messages. 68 | * @return True if correctly initialised, False if already initialised. 69 | */ 70 | virtual bool init(const std::string& param_ns, const graph::core::MetricsPtr& metrics, 71 | const graph::core::CollisionCheckerPtr& checker, const graph::core::SamplerPtr& sampler, 72 | const graph::core::GoalCostFunctionPtr& goal_cost_fcn, const cnr_logger::TraceLoggerPtr& logger) 73 | { 74 | solver_ = std::make_shared(metrics, checker, sampler, goal_cost_fcn, logger); 75 | return true; 76 | } 77 | }; 78 | 79 | } // namespace core 80 | } // namespace graph 81 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/plugins/solvers/tree_solver_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace graph 33 | { 34 | namespace core 35 | { 36 | /** 37 | * @class TreeSolverPlugin 38 | * @brief This class implements a wrapper to graph::core::TreeSolver to allow 39 | * its plugin to be defined. The class can be loaded as a plugin and builds a 40 | * graph::core::TreeSolver object. 41 | */ 42 | class TreeSolverPlugin; 43 | typedef std::shared_ptr TreeSolverPluginPtr; 44 | 45 | class TreeSolverPlugin : public std::enable_shared_from_this 46 | { 47 | protected: 48 | /** 49 | * @brief solver_ is the graph::core::TreeSolver object built and initialized 50 | * by this plugin class. 51 | */ 52 | graph::core::TreeSolverPtr solver_; 53 | 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | /** 58 | * @brief Empty constructor for TreeSolverPlugin. The function init() must be 59 | * called afterwards. 60 | */ 61 | TreeSolverPlugin() 62 | { 63 | solver_ = nullptr; 64 | } 65 | 66 | /** 67 | * @brief Destructor of TreeSolverPlugin. 68 | */ 69 | virtual ~TreeSolverPlugin() 70 | { 71 | solver_ = nullptr; 72 | } 73 | 74 | /** 75 | * @brief getSolver return the graph::core::TreeSolverPtr object built by the 76 | * plugin. 77 | * @return the graph::core::TreeSolverPtr object built. 78 | */ 79 | virtual graph::core::TreeSolverPtr getSolver() 80 | { 81 | return solver_; 82 | } 83 | 84 | /** 85 | * @brief init Initialise the object, defining its main attributes. 86 | * @param param_ns defines the namespace under which parameter are searched 87 | * for using cnr_param library. 88 | * @param metrics The metrics used to evaluate paths. 89 | * @param checker The collision checker for checking collisions. 90 | * @param sampler The sampler for generating random configurations. 91 | * @param goal_cost_fcn The function used to assign the cost of the goal. If 92 | * it is not defined, the default cost function does not assign any cost to 93 | * the goal. 94 | * @param logger The logger for logging messages. 95 | * @return True if correctly initialised, False if already initialised. 96 | */ 97 | virtual bool init(const std::string& param_ns, const graph::core::MetricsPtr& metrics, 98 | const graph::core::CollisionCheckerPtr& checker, const graph::core::SamplerPtr& sampler, 99 | const graph::core::GoalCostFunctionPtr& goal_cost_fcn, 100 | const cnr_logger::TraceLoggerPtr& logger) = 0; 101 | }; 102 | 103 | } // namespace core 104 | } // namespace graph 105 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/samplers/ball_sampler.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace graph 36 | { 37 | namespace core 38 | { 39 | /** 40 | * @class BallSampler 41 | * @brief Sampling in a defined ball. 42 | * 43 | * The BallSampler class inherits from SamplerBase and samples in a ball 44 | * with radius set by "cost" and center provided by input. 45 | */ 46 | class BallSampler; 47 | typedef std::shared_ptr BallSamplerPtr; 48 | 49 | class BallSampler : public SamplerBase 50 | { 51 | protected: 52 | /** 53 | * @brief ball_center_ center of the ball. 54 | */ 55 | Eigen::VectorXd ball_center_; 56 | 57 | // double radius_; //SamplerBase::cost_ is considered as the radius 58 | 59 | /** 60 | * @brief ball_ the ball to sample from. 61 | */ 62 | Eigen::VectorXd ball_; 63 | 64 | /** 65 | * @brief Configure the sampler parameters. 66 | */ 67 | virtual void config(); 68 | 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 71 | 72 | /** 73 | * @brief Empty constructor for BallSampler. The function init() must be 74 | * called afterwards. 75 | */ 76 | BallSampler() : SamplerBase() // set initialized_ false 77 | { 78 | } 79 | 80 | /** 81 | * @brief Constructor for BallSampler. 82 | * @param ball_center Center of the ball. 83 | * @param lower_bound Lower bounds for each dimension. 84 | * @param upper_bound Upper bounds for each dimension. 85 | * @param logger TraceLogger for logging. 86 | * @param radius radius of the ball (default: infinity). 87 | */ 88 | BallSampler(const Eigen::VectorXd& ball_center, const Eigen::VectorXd& lower_bound, 89 | const Eigen::VectorXd& upper_bound, const cnr_logger::TraceLoggerPtr& logger, const double& radius) 90 | : SamplerBase(lower_bound, upper_bound, logger, 91 | radius) 92 | , // set initialized_ true 93 | ball_center_(ball_center) 94 | { 95 | config(); 96 | } 97 | 98 | /** 99 | * @brief init Initialise the object, defining its main attributes. At the end 100 | * of the function, the flag 'initialized_' is set to true and the object can 101 | * execute its main functions. 102 | * @param ball_center Center of the ball. 103 | * @param lower_bound Lower bounds for configuration sampling. 104 | * @param upper_bound Upper bounds for configuration sampling. 105 | * @param logger Pointer to a TraceLogger instance for logging. 106 | * @param radius radius of the ball (default: infinity). 107 | * @return True if correctly initialised, False if already initialised. 108 | */ 109 | virtual bool init(const Eigen::VectorXd& ball_center, const Eigen::VectorXd& lower_bound, 110 | const Eigen::VectorXd& upper_bound, const cnr_logger::TraceLoggerPtr& logger, 111 | const double& radius = std::numeric_limits::infinity()) 112 | { 113 | if (not SamplerBase::init(lower_bound, upper_bound, logger, radius)) 114 | return false; 115 | 116 | ball_center_ = ball_center; 117 | config(); 118 | 119 | return true; 120 | } 121 | 122 | /** 123 | * @brief Generate a sampled configuration. 124 | * @return Sampled configuration. 125 | */ 126 | virtual Eigen::VectorXd sample() override; 127 | 128 | /** 129 | * @brief Check if a configuration is within the bounds. 130 | * @param q Configuration to check. 131 | * @return True if in bounds, false otherwise. 132 | */ 133 | virtual bool inBounds(const Eigen::VectorXd& q) override; 134 | 135 | /** 136 | * @brief Get the ball center of the ball sampler. 137 | * @return Ball center of the ball sampler. 138 | */ 139 | const Eigen::VectorXd& getBallCenter() 140 | { 141 | return ball_center_; 142 | } 143 | 144 | /** 145 | * @brief Set the cost associated with the sampler. 146 | * 147 | * @param cost Cost to be set. 148 | */ 149 | virtual void setCost(const double& cost) override; 150 | 151 | /** 152 | * @brief Check if the informed bounds collapse (focii distance exceeds cost). 153 | * @return True if bounds collapse, false otherwise. 154 | */ 155 | virtual bool collapse() override 156 | { 157 | return false; 158 | } 159 | 160 | /** 161 | * @brief Creates a clone of the BallSampler object. 162 | * @return A shared pointer to the cloned BallSampler object. 163 | */ 164 | virtual SamplerPtr clone() override; 165 | }; 166 | 167 | } // end namespace core 168 | } // end namespace graph 169 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/samplers/uniform_sampler.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace graph 36 | { 37 | namespace core 38 | { 39 | /** 40 | * @class UniformSampler 41 | * @brief Sampling uniformly in the search space. 42 | * 43 | * The UniformSampler class inherits from SamplerBase and samples uniformly 44 | * in the search space considering the lower and upper bounds. 45 | */ 46 | class UniformSampler; 47 | typedef std::shared_ptr UniformSamplerPtr; 48 | 49 | class UniformSampler : public SamplerBase 50 | { 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | 54 | /** 55 | * @brief Empty constructor for UniformSampler. The function init() must be 56 | * called afterwards. 57 | */ 58 | UniformSampler() : SamplerBase() // set initialized_ false 59 | { 60 | } 61 | 62 | /** 63 | * @brief Constructor for UniformSampler. 64 | * @param lower_bound Lower bounds for each dimension. 65 | * @param upper_bound Upper bounds for each dimension. 66 | * @param logger TraceLogger for logging. 67 | */ 68 | UniformSampler(const Eigen::VectorXd& lower_bound, const Eigen::VectorXd& upper_bound, 69 | const cnr_logger::TraceLoggerPtr& logger) 70 | : SamplerBase(lower_bound, upper_bound, logger) // set initialized_ true 71 | { 72 | } 73 | 74 | /** 75 | * @brief Generate a sampled configuration. 76 | * @return Sampled configuration. 77 | */ 78 | virtual Eigen::VectorXd sample() override; 79 | 80 | /** 81 | * @brief Set the cost associated with the sampler. It has no effect on this 82 | * sampler. 83 | * 84 | * @param cost Cost to be set. 85 | */ 86 | virtual void setCost(const double& cost) override 87 | { 88 | cost_ = cost; 89 | } 90 | 91 | /** 92 | * @brief Check if the sampler collapse. It has no effect on this sampler. 93 | * @return True if bounds collapse, false otherwise. 94 | */ 95 | virtual bool collapse() override 96 | { 97 | return false; 98 | } 99 | 100 | /** 101 | * @brief Creates a clone of the UniformSampler object. 102 | * @return A shared pointer to the cloned UniformSampler object. 103 | */ 104 | virtual SamplerPtr clone() override; 105 | }; 106 | 107 | } // end namespace core 108 | } // end namespace graph 109 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/solvers/birrt.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | class BiRRT; 36 | typedef std::shared_ptr BiRRTPtr; 37 | 38 | class BiRRT : public RRT 39 | { 40 | protected: 41 | TreePtr goal_tree_; 42 | 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | 46 | BiRRT() : RRT() 47 | { 48 | } // set initialized_ false 49 | 50 | BiRRT(const MetricsPtr& metrics, const CollisionCheckerPtr& checker, const SamplerPtr& sampler, 51 | const GoalCostFunctionPtr& goal_cost_fcn, const cnr_logger::TraceLoggerPtr& logger) 52 | : RRT(metrics, checker, sampler, goal_cost_fcn, logger) 53 | { 54 | } // set initialized_ true 55 | 56 | BiRRT(const MetricsPtr& metrics, const CollisionCheckerPtr& checker, const SamplerPtr& sampler, 57 | const cnr_logger::TraceLoggerPtr& logger) 58 | : RRT(metrics, checker, sampler, logger) 59 | { 60 | } // set initialized_ true 61 | 62 | virtual bool addGoal(const NodePtr& goal_node, 63 | const double& max_time = std::numeric_limits::infinity()) override; 64 | virtual bool update(PathPtr& solution) override; 65 | virtual bool update(const Eigen::VectorXd& configuration, PathPtr& solution) override; 66 | virtual bool update(const NodePtr& n, PathPtr& solution) override; 67 | 68 | bool importFromSolver(const BiRRTPtr& solver); 69 | bool importFromSolver(const TreeSolverPtr& solver) override; 70 | }; 71 | 72 | } // end namespace core 73 | } // end namespace graph 74 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/solvers/path_optimizers/path_optimizer_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace graph 34 | { 35 | namespace core 36 | { 37 | // class PathOptimizerBase; //Defined in util.h 38 | typedef std::shared_ptr PathOptimizerPtr; 39 | 40 | /** 41 | * @brief Base class for path optimization algorithms, such as local optimizers. 42 | * 43 | * This class serves as the base class for various path optimization algorithms. 44 | * It provides a common interface and structure for different optimization 45 | * approaches. 46 | */ 47 | 48 | class PathOptimizerBase : public std::enable_shared_from_this 49 | { 50 | friend class Path; 51 | 52 | protected: 53 | /** 54 | * @brief path_ The input path to be optimized. 55 | */ 56 | PathPtr path_; 57 | 58 | /** 59 | * @brief checker_ Collision checker for path validity. 60 | */ 61 | CollisionCheckerPtr checker_; 62 | 63 | /** 64 | * @brief metrics_ Metrics used to evaluate the quality of the path. 65 | */ 66 | MetricsPtr metrics_; 67 | 68 | /** 69 | * @brief configured_ Flag indicating whether the optimizer is configured 70 | * ('config' method executed). 71 | */ 72 | bool configured_; 73 | 74 | /** 75 | * @brief stall_gen_ Counter for stalled generations. 76 | */ 77 | unsigned int stall_gen_; 78 | 79 | /** 80 | * @brief max_stall_gen_ Maximum number of stalled generations allowed. 81 | */ 82 | unsigned int max_stall_gen_; 83 | 84 | /** 85 | * @brief solved_ Flag indicating whether the optimization process has found a 86 | * solution. 87 | */ 88 | bool solved_; 89 | 90 | /** 91 | * @brief The namespace under which the parameters are searched for. 92 | */ 93 | std::string param_ns_; 94 | 95 | /** 96 | * @brief Pointer to a TraceLogger instance for logging. 97 | * 98 | * This member variable represents a pointer to a TraceLogger instance, 99 | * allowing to perform logging operations. TraceLogger is a part of the 100 | * cnr_logger library. Ensure that the logger is properly configured and 101 | * available for use. 102 | */ 103 | cnr_logger::TraceLoggerPtr logger_; 104 | 105 | /** 106 | * @brief Perform one optimization step. Derived classes must implement this 107 | * function. 108 | * @return True if the optimization step was successful, false otherwise. 109 | */ 110 | virtual bool step() = 0; 111 | 112 | public: 113 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 114 | 115 | /** 116 | * @brief Constructor for PathOptimizerBase. 117 | * @param checker Collision checker for path validity. 118 | * @param metrics Metrics used to evaluate the quality of the path. 119 | * @param logger Logger for debugging and logging. 120 | */ 121 | PathOptimizerBase(const CollisionCheckerPtr& checker, const MetricsPtr& metrics, 122 | const cnr_logger::TraceLoggerPtr& logger); 123 | 124 | /** 125 | * @brief Set the input path for optimization and reset the flags 'solved_' 126 | * and 'stall_gen_'. 127 | * @param path The input path to be optimized. 128 | */ 129 | virtual void setPath(const PathPtr& path); 130 | 131 | /** 132 | * @brief Get the optimized path. 133 | * @return The shared pointer to the optimized path. 134 | */ 135 | virtual PathPtr getPath(); 136 | 137 | /** 138 | * @brief Configure the optimizer reading parameters with cnr_param library. 139 | */ 140 | virtual void config(const std::string& param_ns); 141 | 142 | /** 143 | * @brief Solve the optimization problem. Use 'getPath' to get the processed 144 | * path. 145 | * @param max_iteration Maximum number of iterations for the optimization 146 | * process. 147 | * @param max_time Maximum time (in seconds) allowed for the optimization 148 | * process. 149 | * @return True if a solution was found, false otherwise. 150 | */ 151 | virtual bool solve(const unsigned int& max_iteration = 100, 152 | const double& max_time = std::numeric_limits::infinity()); 153 | }; 154 | 155 | } // end namespace core 156 | } // end namespace graph 157 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/solvers/rrt.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | class RRT; 36 | typedef std::shared_ptr RRTPtr; 37 | 38 | class RRT : public TreeSolver 39 | { 40 | protected: 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | 44 | RRT() : TreeSolver() 45 | { 46 | } // set initialized_ false 47 | 48 | RRT(const MetricsPtr& metrics, const CollisionCheckerPtr& checker, const SamplerPtr& sampler, 49 | const GoalCostFunctionPtr& goal_cost_fcn, const cnr_logger::TraceLoggerPtr& logger) 50 | : TreeSolver(metrics, checker, sampler, goal_cost_fcn, logger) 51 | { 52 | } // set initialized_ true 53 | 54 | RRT(const MetricsPtr& metrics, const CollisionCheckerPtr& checker, const SamplerPtr& sampler, 55 | const cnr_logger::TraceLoggerPtr& logger) 56 | : TreeSolver(metrics, checker, sampler, logger) 57 | { 58 | } // set initialized_ true 59 | 60 | virtual bool addStart(const NodePtr& start_node, 61 | const double& max_time = std::numeric_limits::infinity()) override; 62 | virtual bool addStartTree(const TreePtr& start_tree, 63 | const double& max_time = std::numeric_limits::infinity()) override; 64 | virtual bool addGoal(const NodePtr& goal_node, 65 | const double& max_time = std::numeric_limits::infinity()) override; 66 | virtual void resetProblem() override; 67 | virtual bool update(const Eigen::VectorXd& configuration, PathPtr& solution) override; 68 | virtual bool update(const NodePtr& n, PathPtr& solution) override; 69 | virtual bool update(PathPtr& solution) override; 70 | }; 71 | 72 | } // end namespace core 73 | } // end namespace graph 74 | -------------------------------------------------------------------------------- /graph_core/include/graph_core/solvers/rrt_star.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /* 3 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 4 | manuel.beschi@unibs.it, c.tonola001@unibs.it 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | 31 | namespace graph 32 | { 33 | namespace core 34 | { 35 | class RRTStar; 36 | typedef std::shared_ptr RRTStarPtr; 37 | 38 | class RRTStar : public RRT 39 | { 40 | protected: 41 | /** 42 | * @brief The rewiring factor, r_rrt = rewire_factor_ \times r_rrt* > r_rrt* 43 | */ 44 | double rewire_factor_ = 1.1; 45 | 46 | /** 47 | * @brief r_rewire_ the rewire radius 48 | */ 49 | double r_rewire_; 50 | 51 | /** 52 | * @brief Update the rewire radius. 53 | * 54 | * This function calculates the rewire radius based on the dimensionality 55 | * of the problem space and the specific volume of the sampler. 56 | * 57 | * @param sampler A shared pointer to a sampler object. 58 | */ 59 | virtual void updateRewireRadius(); 60 | virtual void updateRewireRadius(const SamplerPtr& sampler); 61 | 62 | public: 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | 65 | RRTStar() : RRT() 66 | { 67 | } // set initialized_ false 68 | 69 | RRTStar(const MetricsPtr& metrics, const CollisionCheckerPtr& checker, const SamplerPtr& sampler, 70 | const GoalCostFunctionPtr& goal_cost_fcn, const cnr_logger::TraceLoggerPtr& logger) 71 | : RRT(metrics, checker, sampler, goal_cost_fcn, logger) 72 | { 73 | } // set initialized_ true 74 | 75 | RRTStar(const MetricsPtr& metrics, const CollisionCheckerPtr& checker, const SamplerPtr& sampler, 76 | const cnr_logger::TraceLoggerPtr& logger) 77 | : RRT(metrics, checker, sampler, logger) 78 | { 79 | } // set initialized_ true 80 | 81 | virtual bool config(const std::string& param_ns) override; 82 | virtual bool addStartTree(const TreePtr& start_tree, 83 | const double& max_time = std::numeric_limits::infinity()) override; 84 | virtual bool update(PathPtr& solution) override; 85 | virtual bool solve(PathPtr& solution, const unsigned int& max_iter = 100, 86 | const double& max_time = std::numeric_limits::infinity()) override; 87 | virtual bool update(const Eigen::VectorXd& configuration, PathPtr& solution) override; 88 | virtual bool update(const NodePtr& n, PathPtr& solution) override; 89 | 90 | double getRewireRadius() 91 | { 92 | return r_rewire_; 93 | } 94 | 95 | bool importFromSolver(const RRTStarPtr& solver); 96 | bool importFromSolver(const TreeSolverPtr& solver) override; 97 | }; 98 | 99 | } // end namespace core 100 | } // end namespace graph 101 | -------------------------------------------------------------------------------- /graph_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graph_core 4 | 0.0.0 5 | The graph_core package 6 | 7 | Cesare Tonola 8 | Manuel Beschi 9 | Marco Faroni 10 | 11 | BSD-3-Clause 12 | 13 | Cesare Tonola 14 | 15 | catkin 16 | cmake 17 | 18 | eigen 19 | cnr_param 20 | cnr_logger 21 | cnr_class_loader 22 | 23 | 24 | catkin 25 | cmake 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/datastructure/vector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | namespace graph 31 | { 32 | namespace core 33 | { 34 | Vector::Vector(const cnr_logger::TraceLoggerPtr& logger) : NearestNeighbors(logger) 35 | { 36 | } 37 | 38 | void Vector::insert(const NodePtr& node) 39 | { 40 | nodes_.push_back(node); 41 | size_++; 42 | return; 43 | } 44 | 45 | bool Vector::clear() 46 | { 47 | size_ = 0; 48 | deleted_nodes_ = 0; 49 | 50 | nodes_.clear(); 51 | 52 | return true; 53 | } 54 | 55 | void Vector::nearestNeighbor(const Eigen::VectorXd& configuration, NodePtr& best, double& best_distance) 56 | { 57 | best_distance = std::numeric_limits::infinity(); 58 | for (const NodePtr& n : nodes_) 59 | { 60 | double dist = (n->getConfiguration() - configuration).norm(); 61 | if (dist < best_distance) 62 | { 63 | best = n; 64 | best_distance = dist; 65 | } 66 | } 67 | } 68 | 69 | std::multimap Vector::near(const Eigen::VectorXd& configuration, const double& radius) 70 | { 71 | std::multimap nodes; 72 | for (const NodePtr& n : nodes_) 73 | { 74 | double dist = (n->getConfiguration() - configuration).norm(); 75 | if (dist < radius) 76 | { 77 | nodes.insert(std::pair(dist, n)); 78 | } 79 | } 80 | return nodes; 81 | } 82 | 83 | std::multimap Vector::kNearestNeighbors(const Eigen::VectorXd& configuration, const size_t& k) 84 | { 85 | std::multimap nodes; 86 | for (const NodePtr& n : nodes_) 87 | { 88 | double dist = (n->getConfiguration() - configuration).norm(); 89 | nodes.insert(std::pair(dist, n)); 90 | } 91 | if (nodes.size() < k) 92 | return nodes; 93 | 94 | std::multimap m2(nodes.begin(), std::next(nodes.begin(), k)); 95 | return m2; 96 | } 97 | 98 | bool Vector::findNode(const NodePtr& node) 99 | { 100 | return (std::find(nodes_.begin(), nodes_.end(), node) != nodes_.end()); 101 | } 102 | 103 | bool Vector::deleteNode(const NodePtr& node, const bool& disconnect_node) 104 | { 105 | std::vector::iterator it; 106 | it = std::find(nodes_.begin(), nodes_.end(), node); 107 | if (it == nodes_.end()) 108 | return false; 109 | 110 | size_--; 111 | deleted_nodes_++; 112 | 113 | nodes_.erase(it); 114 | 115 | if (disconnect_node) 116 | node->disconnect(); 117 | 118 | return true; 119 | } 120 | 121 | bool Vector::restoreNode(const NodePtr& node) 122 | { 123 | return false; 124 | } 125 | 126 | std::vector Vector::getNodes() 127 | { 128 | return nodes_; 129 | } 130 | 131 | void Vector::disconnectNodes(const std::vector& white_list) 132 | { 133 | for (NodePtr& n : nodes_) 134 | { 135 | if (std::find(white_list.begin(), white_list.end(), n) == white_list.end()) 136 | n->disconnect(); 137 | } 138 | } 139 | 140 | } // end namespace core 141 | } // end namespace graph 142 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/metrics/euclidean_metrics.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | #include 28 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/metrics/euclidean_metrics_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::EuclideanMetricsPlugin, graph::core::MetricsBasePlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/metrics/null_goal_cost_function_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::NullGoalCostFunctionPlugin, graph::core::GoalCostFunctionBasePlugin) 33 | 34 | namespace graph 35 | { 36 | namespace core 37 | { 38 | bool NullGoalCostFunctionPlugin::init(const std::string& param_ns, const cnr_logger::TraceLoggerPtr& logger) 39 | { 40 | CNR_TRACE(logger, "creating NullGoalCostFunction using namespace << " << param_ns); 41 | goal_cost_fcn_ = std::make_shared(logger); 42 | CNR_TRACE(logger, "created NullGoalCostFunction"); 43 | return true; 44 | } 45 | 46 | } // namespace core 47 | } // namespace graph 48 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/samplers/ball_sampler_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::BallSamplerPlugin, graph::core::SamplerBasePlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/samplers/informed_sampler_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::InformedSamplerPlugin, graph::core::SamplerBasePlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/samplers/uniform_sampler_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::UniformSamplerPlugin, graph::core::SamplerBasePlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/solvers/anytime_rrt_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::AnytimeRRTPlugin, graph::core::TreeSolverPlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/solvers/birrt_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::BiRRTPlugin, graph::core::TreeSolverPlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/solvers/rrt_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | /** 30 | * @brief Register class to be loaded with cnr_class_loader 31 | */ 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::RRTPlugin, graph::core::TreeSolverPlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/plugins/solvers/rrt_star_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | /** 29 | * @brief Register class to be loaded with cnr_class_loader 30 | */ 31 | #include 32 | CLASS_LOADER_REGISTER_CLASS(graph::core::RRTStarPlugin, graph::core::TreeSolverPlugin) 33 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/samplers/ball_sampler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | namespace graph 31 | { 32 | namespace core 33 | { 34 | void BallSampler::config() 35 | { 36 | if (not initialized_) 37 | { 38 | CNR_ERROR(logger_, "Ball sampler not initialised, cannot configure"); 39 | return; 40 | } 41 | 42 | if (cost_ < 0.0) 43 | { 44 | CNR_FATAL(logger_, "cost should be >= 0"); 45 | throw std::invalid_argument("cost should be >= 0"); 46 | } 47 | 48 | if (ball_center_.rows() != ndof_) 49 | { 50 | CNR_FATAL(logger_, "ball center should have the same size of ndof"); 51 | throw std::invalid_argument("ball center should have the same size of ndof"); 52 | } 53 | 54 | ball_.setRandom(ndof_, 1); 55 | } 56 | 57 | Eigen::VectorXd BallSampler::sample() 58 | { 59 | if (cost_ < std::numeric_limits::infinity()) 60 | { 61 | for (int itrial = 0; itrial < 100; itrial++) 62 | { 63 | ball_.setRandom(ndof_, 1); 64 | ball_ *= std::pow(ud_(gen_), 1.0 / (double)ndof_) / ball_.norm(); 65 | 66 | Eigen::VectorXd q = cost_ * ball_ + ball_center_; 67 | if (inBounds(q)) 68 | return q; 69 | } 70 | 71 | CNR_WARN(logger_, 72 | "BallSampler has not found a sample in the ball that " 73 | "respects the bounds"); 74 | } 75 | 76 | // Sample everywhere 77 | return 0.5 * (lower_bound_ + upper_bound_) + 78 | Eigen::MatrixXd::Random(ndof_, 1).cwiseProduct(0.5 * (lower_bound_ - upper_bound_)); 79 | } 80 | 81 | bool BallSampler::inBounds(const Eigen::VectorXd& q) 82 | { 83 | for (unsigned int iax = 0; iax < ndof_; iax++) 84 | { 85 | if (q(iax) > upper_bound_(iax) || q(iax) < lower_bound_(iax)) 86 | { 87 | return false; 88 | } 89 | } 90 | if (cost_ == std::numeric_limits::infinity()) 91 | return true; 92 | else 93 | return (q - ball_center_).norm() < cost_; 94 | } 95 | 96 | void BallSampler::setCost(const double& cost) 97 | { 98 | if (cost_ < 0.0) 99 | { 100 | CNR_FATAL(logger_, "cost should be >= 0"); 101 | throw std::invalid_argument("cost should be >= 0"); 102 | } 103 | cost_ = cost; 104 | } 105 | 106 | SamplerPtr BallSampler::clone() 107 | { 108 | return std::make_shared(ball_center_, lower_bound_, upper_bound_, logger_, cost_); 109 | } 110 | 111 | } // end namespace core 112 | } // end namespace graph 113 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/samplers/tube_informed_sampler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | namespace graph 31 | { 32 | namespace core 33 | { 34 | bool TubeInformedSampler::init(const SamplerPtr& sampler, const MetricsPtr& metrics) 35 | { 36 | if (not SamplerBase::init(sampler->getLB(), sampler->getUB(), sampler->getLogger(), sampler->getCost())) 37 | return false; 38 | 39 | length_ = 0; 40 | radius_ = 0; 41 | sampler_ = sampler; 42 | metrics_ = metrics; 43 | 44 | return true; 45 | } 46 | 47 | Eigen::VectorXd TubeInformedSampler::sample() 48 | { 49 | if (ud_(gen_) > local_bias_) 50 | return sampler_->sample(); 51 | 52 | if (length_ <= 0) 53 | return sampler_->sample(); 54 | 55 | for (int itrial = 0; itrial < 100; itrial++) 56 | { 57 | double abscissa = ud_(gen_) * length_; 58 | Eigen::VectorXd center = pointOnCurvilinearAbscissa(abscissa); 59 | Eigen::VectorXd ball(ndof_); 60 | ball.setRandom(); 61 | ball *= std::pow(ud_(gen_), 1.0 / (double)ndof_) / ball.norm(); 62 | Eigen::VectorXd q = radius_ * ball + center; 63 | if (sampler_->inBounds(q) && couldImprove(q)) 64 | { 65 | return q; 66 | } 67 | } 68 | return sampler_->sample(); 69 | } 70 | 71 | bool TubeInformedSampler::setPath(const PathPtr& path) 72 | { 73 | return setPath(path->getWaypoints()); 74 | } 75 | 76 | bool TubeInformedSampler::setPath(const std::vector& path) 77 | { 78 | if (path.size() == 0) 79 | return false; 80 | 81 | path_ = path; 82 | partial_length_.resize(path.size(), 0); 83 | partial_cost_.resize(path.size(), 0); 84 | for (size_t idx = 1; idx < path.size(); idx++) 85 | { 86 | partial_length_.at(idx) = partial_length_.at(idx - 1) + (path.at(idx) - path.at(idx - 1)).norm(); 87 | partial_cost_.at(idx) = partial_cost_.at(idx - 1) + metrics_->utopia(path.at(idx - 1), path.at(idx)); 88 | } 89 | length_ = partial_length_.back(); 90 | return length_ > 0; 91 | } 92 | 93 | bool TubeInformedSampler::setPath(const std::vector>& path) 94 | { 95 | std::vector eigen_path(path.size()); 96 | for (size_t idx = 0; idx < path.size(); idx++) 97 | { 98 | eigen_path.at(idx).resize(path.at(idx).size()); 99 | for (unsigned iax = 0; iax < path.at(idx).size(); iax++) 100 | eigen_path.at(idx)(iax) = path.at(idx).at(iax); 101 | } 102 | return setPath(eigen_path); 103 | } 104 | 105 | bool TubeInformedSampler::setRadius(const double& radius) 106 | { 107 | if (radius <= 0) 108 | { 109 | CNR_WARN(logger_, "Radius should be positive"); 110 | return false; 111 | } 112 | radius_ = radius; 113 | return true; 114 | } 115 | 116 | bool TubeInformedSampler::setLocalBias(const double& local_bias) 117 | { 118 | if ((local_bias < 0) || (local_bias > 1)) 119 | { 120 | CNR_WARN(logger_, "Local bias should be between 0-1"); 121 | return false; 122 | } 123 | local_bias_ = local_bias; 124 | return true; 125 | } 126 | 127 | Eigen::VectorXd TubeInformedSampler::pointOnCurvilinearAbscissa(const double& abscissa) 128 | { 129 | if (abscissa <= 0) 130 | return path_.at(0); 131 | else if (abscissa >= length_) 132 | return path_.back(); 133 | 134 | for (size_t idx = 1; idx < path_.size(); idx++) 135 | { 136 | if (partial_length_.at(idx) > abscissa) 137 | { 138 | double ratio = (abscissa - partial_length_.at(idx - 1)) / (partial_length_.at(idx) - partial_length_.at(idx - 1)); 139 | return path_.at(idx - 1) + ratio * (path_.at(idx) - path_.at(idx - 1)); 140 | } 141 | } 142 | return path_.back(); 143 | } 144 | 145 | void TubeInformedSampler::setCost(const double& cost) 146 | { 147 | cost_ = cost; 148 | sampler_->setCost(cost); 149 | } 150 | 151 | bool TubeInformedSampler::collapse() 152 | { 153 | return sampler_->collapse(); 154 | } 155 | 156 | bool TubeInformedSampler::couldImprove(const Eigen::VectorXd& q) 157 | { 158 | for (size_t idx = 1; idx < path_.size() - 1; idx++) 159 | { 160 | double delta_cost = partial_cost_.at(idx + 1) - partial_cost_.at(idx - 1); 161 | double test_cost = metrics_->utopia(q, path_.at(idx + 1)) + metrics_->utopia(path_.at(idx - 1), q); 162 | if (test_cost < delta_cost) 163 | return true; 164 | } 165 | return false; 166 | } 167 | 168 | SamplerPtr TubeInformedSampler::clone() 169 | { 170 | return std::make_shared(sampler_->clone(), metrics_->clone()); 171 | } 172 | 173 | } // end namespace core 174 | } // end namespace graph 175 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/samplers/uniform_sampler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Cesare Tonola University of Brescia c.tonola001@unibs.it 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | namespace graph 31 | { 32 | namespace core 33 | { 34 | Eigen::VectorXd UniformSampler::sample() 35 | { 36 | // Sample everywhere 37 | return 0.5 * (lower_bound_ + upper_bound_) + 38 | Eigen::MatrixXd::Random(ndof_, 1).cwiseProduct(0.5 * (lower_bound_ - upper_bound_)); 39 | } 40 | 41 | SamplerPtr UniformSampler::clone() 42 | { 43 | return std::make_shared(lower_bound_, upper_bound_, logger_); 44 | } 45 | 46 | } // end namespace core 47 | } // end namespace graph 48 | -------------------------------------------------------------------------------- /graph_core/src/graph_core/solvers/path_optimizers/path_optimizer_base.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2024, Manuel Beschi and Cesare Tonola, JRL-CARI CNR-STIIMA/UNIBS, 3 | manuel.beschi@unibs.it, c.tonola001@unibs.it All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | namespace graph 31 | { 32 | namespace core 33 | { 34 | PathOptimizerBase::PathOptimizerBase(const CollisionCheckerPtr& checker, const MetricsPtr& metrics, 35 | const cnr_logger::TraceLoggerPtr& logger) 36 | : checker_(checker), metrics_(metrics), logger_(logger) 37 | { 38 | solved_ = false; 39 | configured_ = false; 40 | } 41 | 42 | void PathOptimizerBase::config(const std::string& param_ns) 43 | { 44 | param_ns_ = param_ns; 45 | get_param(logger_, param_ns_, "max_stall_gen", max_stall_gen_, (unsigned int)10); 46 | 47 | stall_gen_ = 0; 48 | configured_ = true; 49 | } 50 | 51 | void PathOptimizerBase::setPath(const PathPtr& path) 52 | { 53 | if (not path) 54 | { 55 | CNR_WARN(logger_, "path is null"); 56 | return; 57 | } 58 | 59 | solved_ = false; 60 | stall_gen_ = 0; 61 | path_ = path; 62 | } 63 | 64 | PathPtr PathOptimizerBase::getPath() 65 | { 66 | return path_; 67 | } 68 | 69 | bool PathOptimizerBase::solve(const unsigned int& max_iteration, const double& max_time) 70 | { 71 | if (not configured_) 72 | { 73 | CNR_ERROR(logger_, "Path local solver not configured!"); 74 | return false; 75 | } 76 | 77 | auto tic = graph_time::now(); 78 | if (max_time <= 0.0) 79 | return false; 80 | 81 | unsigned int iter = 0; 82 | while (iter++ < max_iteration) 83 | { 84 | if (solved_) 85 | { 86 | CNR_DEBUG(logger_, "solved in %u iterations", iter); 87 | return true; 88 | } 89 | step(); 90 | 91 | if (toSeconds(graph_time::now(), tic) >= 0.98 * max_time) 92 | break; 93 | } 94 | return solved_; 95 | } 96 | 97 | } // end namespace core 98 | } // end namespace graph 99 | -------------------------------------------------------------------------------- /graph_core/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(kdtree_test src/kdtree_test.cpp) 2 | target_compile_definitions(kdtree_test 3 | PRIVATE 4 | TEST_DIR="${CMAKE_CURRENT_LIST_DIR}") 5 | target_link_libraries(kdtree_test PUBLIC 6 | graph_core::graph_core 7 | ) 8 | install(TARGETS kdtree_test RUNTIME DESTINATION "bin/graph_core/tests/src") 9 | 10 | add_executable(node_connection_test src/node_connection_test.cpp) 11 | target_compile_definitions(node_connection_test 12 | PRIVATE 13 | TEST_DIR="${CMAKE_CURRENT_LIST_DIR}") 14 | target_link_libraries(node_connection_test PUBLIC 15 | graph_core::graph_core 16 | ) 17 | install(TARGETS node_connection_test RUNTIME DESTINATION "bin/graph_core/tests/src") 18 | 19 | add_executable(compute_path_test src/compute_path_test.cpp) 20 | target_compile_definitions(compute_path_test 21 | PRIVATE 22 | TEST_DIR="${CMAKE_CURRENT_LIST_DIR}") 23 | target_link_libraries(compute_path_test PUBLIC 24 | graph_core::graph_core 25 | ) 26 | install(TARGETS compute_path_test RUNTIME DESTINATION "bin/graph_core/tests/src") 27 | 28 | add_executable(path_post_processing_test src/path_post_processing_test.cpp) 29 | target_compile_definitions(path_post_processing_test 30 | PRIVATE 31 | TEST_DIR="${CMAKE_CURRENT_LIST_DIR}") 32 | target_link_libraries(path_post_processing_test PUBLIC 33 | graph_core::graph_core 34 | ) 35 | install(TARGETS path_post_processing_test RUNTIME DESTINATION "bin/graph_core/tests/src") 36 | 37 | #GTests 38 | find_package(GTest REQUIRED) 39 | include_directories(${GTEST_INCLUDE_DIRS}) 40 | 41 | add_executable(run_all_tests src/run_all_tests.cpp) 42 | target_link_libraries(run_all_tests PRIVATE ${GTEST_LIBRARIES} pthread graph_core::graph_core) 43 | install(TARGETS run_all_tests RUNTIME DESTINATION "bin/graph_core/tests/src") 44 | 45 | # Install files 46 | install(FILES logger_param.yaml logger_param_gtest.yaml 47 | DESTINATION "bin/graph_core/tests") 48 | -------------------------------------------------------------------------------- /graph_core/tests/graph_core_tests_params.yaml: -------------------------------------------------------------------------------- 1 | graph_core_tests: 2 | min_cc_distance: 0.01 3 | abs_joint_threshold: 1.0 4 | max_distance: 0.25 5 | use_kdtree: true 6 | extend: false 7 | utopia_tolerance: 0.01 8 | max_time: 5.0 9 | rewire_factor: 1.1 10 | simplify_max_conn_length: 0.05 11 | warp_min_conn_length: 0.1 12 | warp_min_step_size: 0.05 13 | -------------------------------------------------------------------------------- /graph_core/tests/logger_param.yaml: -------------------------------------------------------------------------------- 1 | appenders: ["file", "screen"] 2 | levels: ["debug", "debug"] 3 | file_name: "graph_core_tests" -------------------------------------------------------------------------------- /graph_core/tests/logger_param_gtest.yaml: -------------------------------------------------------------------------------- 1 | appenders: ["file"] 2 | levels: ["debug"] 3 | file_name: "graph_core_gtests" -------------------------------------------------------------------------------- /graph_core/tests/path.yaml: -------------------------------------------------------------------------------- 1 | graph_core_tests: 2 | path: 3 | - [-1.5, -1.5, -1.5] 4 | - [-1.368304943282731, -0.869310126335348, -0.7325118657666546] 5 | - [-1.113252883580279, -0.09815786150562764, 0.5658478923234942] 6 | - [-0.986172691213979, 0.05930640853909153, 1.050255684438281] 7 | - [-0.7429864148673528, 0.2486760979916678, 1.160513419597364] 8 | - [-0.2367755061652399, 0.5426347130176773, 1.333123798870073] 9 | - [0.221342196558296, 0.8029216822716043, 1.401807888830923] 10 | - [0.3617322458241751, 0.8665115658037883, 1.374848947802023] 11 | - [0.5586325554450194, 1.018791281859759, 1.367347763789514] 12 | - [0.6556395875083467, 1.103812723236071, 1.374659214110421] 13 | - [0.8294250552213867, 1.236146434087375, 1.394957294173053] 14 | - [1.024025496339438, 1.352149415226723, 1.435204434178399] 15 | - [1.284559647918008, 1.429619306665668, 1.478383608152337] 16 | - [1.5, 1.5, 1.5] -------------------------------------------------------------------------------- /graph_core/tests/src/compute_path_test.cpp: -------------------------------------------------------------------------------- 1 | // Graph core required headers 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char** argv) 8 | { 9 | // Load the logger's configuration 10 | std::string path_to_config_folder = std::string(TEST_DIR); 11 | std::string logger_file = path_to_config_folder + "/logger_param.yaml"; 12 | 13 | if (argc > 1) 14 | logger_file = path_to_config_folder + "/" + std::string(argv[1]); // or take it from argument 15 | 16 | cnr_logger::TraceLoggerPtr logger = std::make_shared("compute_path_test", logger_file); 17 | 18 | // Load params 19 | std::string command = 20 | "cnr_param_server --path-to-file " + std::string(TEST_DIR) + 21 | "/graph_core_tests_params.yaml"; // use cnr_param utility to write parameters contained in this file 22 | CNR_INFO(logger, "Executing command: " << command); 23 | int ret_code = std::system(command.c_str()); 24 | if (ret_code != 0) 25 | { 26 | CNR_ERROR(logger, "Error: command " << command << " failed"); 27 | return ret_code; 28 | } 29 | 30 | // Define namespace for parameters retrieving. Parameters are defined and will be searched under this namespace 31 | std::string param_ns_base = "graph_core_tests"; 32 | std::string param_ns = "/" + param_ns_base; // must begin with "/" 33 | 34 | // Define the collision checker (foo collision checker) 35 | double min_cc_distance; // distance between two configurations to be checked along a connection 36 | double default_min_cc_distance = 0.01; 37 | graph::core::get_param(logger, param_ns, "min_cc_distance", min_cc_distance, 38 | default_min_cc_distance); // wrapper to cnr_param functions 39 | 40 | double abs_joint_threshold; // parameter specific for this foo collision checker: if a joint absolute value is less 41 | // than this threshold, the configuration is considered not valid 42 | double default_abs_joint_threshold = 1.0; 43 | graph::core::get_param(logger, param_ns, "abs_joint_threshold", abs_joint_threshold, default_abs_joint_threshold); 44 | 45 | if (min_cc_distance <= 0) 46 | min_cc_distance = default_min_cc_distance; 47 | 48 | if (abs_joint_threshold <= 0) 49 | abs_joint_threshold = default_abs_joint_threshold; 50 | 51 | graph::core::CollisionCheckerPtr collision_checker = 52 | std::make_shared(logger, abs_joint_threshold, min_cc_distance); 53 | 54 | // Define a cost function (Euclidean metrics) 55 | graph::core::MetricsPtr metrics = std::make_shared(logger); 56 | 57 | // Define lower/upper bounds 58 | size_t dof = 3; 59 | Eigen::VectorXd lb(dof); 60 | lb.setConstant(-2.5); 61 | Eigen::VectorXd ub(dof); 62 | ub.setConstant(2.5); 63 | 64 | // Define a sampler (uniform sampler) 65 | graph::core::SamplerPtr sampler = std::make_shared(lb, ub, logger); 66 | 67 | // Define the solver (RRT*) 68 | graph::core::TreeSolverPtr solver = 69 | std::make_shared(metrics, collision_checker, sampler, logger); 70 | 71 | // Define start and goal nodes 72 | Eigen::VectorXd start_configuration(dof); 73 | start_configuration << -1.5, -1.5, -1.5; 74 | 75 | Eigen::VectorXd goal_configuration(dof); 76 | goal_configuration << 1.5, 1.5, 1.5; 77 | 78 | // Optionally, you can load these directly from params 79 | // graph::core::get_param(logger,param_ns,"start_configuration",start_configuration); 80 | // graph::core::get_param(logger,param_ns,"goal_configuration",goal_configuration) 81 | 82 | graph::core::NodePtr start_node = std::make_shared(start_configuration, logger); 83 | graph::core::NodePtr goal_node = std::make_shared(goal_configuration, logger); 84 | 85 | // Compute a path 86 | double max_time; // seconds 87 | double default_max_time = 5.0; 88 | graph::core::get_param(logger, param_ns, "max_time", max_time, default_max_time); 89 | 90 | size_t max_iter = 1000000; 91 | graph::core::PathPtr solution; 92 | 93 | graph::core::graph_time_point tic = graph::core::graph_time::now(); 94 | bool found = solver->computePath(start_node, goal_node, param_ns, solution, max_time, max_iter); 95 | graph::core::graph_time_point toc = graph::core::graph_time::now(); 96 | 97 | // Print out the solution and save it to a file 98 | double elapsed_time = graph::core::toSeconds(toc, tic); 99 | CNR_INFO(logger, "Elapsed time: " << elapsed_time << " seconds"); 100 | 101 | if (found) 102 | CNR_INFO(logger, cnr_logger::BOLDGREEN() << "Path found!\n" << *solution << cnr_logger::RESET()); 103 | else 104 | CNR_INFO(logger, cnr_logger::BOLDRED() << "Path not found!" << cnr_logger::RESET()); 105 | 106 | YAML::Node yaml_path, yaml_tree, yaml_path_ns, yaml_tree_ns; 107 | yaml_path["path"] = solution->toYAML(); 108 | yaml_tree["tree"] = solution->getTree()->toYAML(); 109 | 110 | yaml_path_ns[param_ns_base] = yaml_path; 111 | yaml_tree_ns[param_ns_base] = yaml_tree; 112 | 113 | std::ofstream fout_path(path_to_config_folder + "/path.yaml"); 114 | std::ofstream fout_tree(path_to_config_folder + "/tree.yaml"); 115 | 116 | if (fout_path.is_open()) 117 | { 118 | fout_path << yaml_path_ns; 119 | fout_path.close(); 120 | } 121 | else 122 | CNR_ERROR(logger, "Error opening 'path.yaml' for writing."); 123 | 124 | if (fout_tree.is_open()) 125 | { 126 | fout_tree << yaml_tree_ns; 127 | fout_tree.close(); 128 | } 129 | else 130 | CNR_ERROR(logger, "Error opening 'tree.yaml' for writing."); 131 | 132 | return 0; 133 | } 134 | -------------------------------------------------------------------------------- /graph_core/tests/src/kdtree_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | // Load the logger's configuration 8 | std::string path_to_config_folder = std::string(TEST_DIR); 9 | std::string logger_file = path_to_config_folder + "/logger_param.yaml"; 10 | 11 | if (argc > 1) 12 | logger_file = path_to_config_folder + "/" + std::string(argv[1]); // or take it from argument 13 | 14 | cnr_logger::TraceLoggerPtr logger = std::make_shared("graph_core_tests_logger", logger_file); 15 | 16 | int n_kdnodes = 10; 17 | int threshold = std::numeric_limits::max(); 18 | 19 | if (argc == 3) 20 | n_kdnodes = std::atoi(argv[2]); 21 | 22 | if (argc == 4) 23 | { 24 | n_kdnodes = std::atoi(argv[2]); 25 | threshold = std::atoi(argv[3]); 26 | } 27 | 28 | if (argc > 4) 29 | CNR_WARN(logger, 30 | cnr_logger::RESET() << cnr_logger::BOLDYELLOW() << "Number of inputs to the program should be <=3"); 31 | 32 | graph::core::KdTreePtr kdtree = std::make_shared(logger); 33 | kdtree->deletedNodesThreshold(threshold); 34 | 35 | std::vector nodes; 36 | for (int i = 0; i < n_kdnodes; i++) 37 | { 38 | Eigen::VectorXd q(3); 39 | q.setRandom(); 40 | graph::core::NodePtr node = std::make_shared(q, logger); 41 | nodes.push_back(node); 42 | 43 | CNR_INFO(logger, 44 | cnr_logger::RESET() << cnr_logger::WHITE() << " Random node -> " << q.transpose() << " (" << node << ")"); 45 | 46 | kdtree->insert(node); 47 | } 48 | 49 | CNR_INFO(logger, cnr_logger::RESET() << cnr_logger::YELLOW() << " KdTree: \n" << *kdtree); 50 | 51 | std::mt19937 rng(std::random_device{}()); 52 | std::uniform_int_distribution dist(0, n_kdnodes - 1); 53 | int n1, n2, n3; 54 | n1 = dist(rng); 55 | n2 = n3 = n1; 56 | while (n2 == n1) 57 | n2 = dist(rng); 58 | while (n3 == n1 || n3 == n2) 59 | n3 = dist(rng); 60 | 61 | graph::core::KdNodePtr kdn1, kdn2, kdn3; 62 | kdtree->findNode(nodes.at(n1), kdn1); 63 | kdtree->findNode(nodes.at(n2), kdn2); 64 | kdtree->findNode(nodes.at(n3), kdn3); 65 | 66 | CNR_INFO(logger, cnr_logger::RESET() << "Removing 3 nodes:\n\t-" << kdn1 << "\n\t-" << kdn2 << "\n\t-" << kdn3); 67 | kdtree->deleteNode(nodes.at(n1)); 68 | kdtree->deleteNode(nodes.at(n2)); 69 | kdtree->deleteNode(nodes.at(n3)); 70 | 71 | CNR_INFO(logger, cnr_logger::RESET() << cnr_logger::YELLOW() << " KdTree: \n" << *kdtree); 72 | kdtree->print_deleted_nodes_ = true; 73 | CNR_INFO(logger, cnr_logger::RESET() << cnr_logger::BLUE() << " KdTree with also deleted nodes: \n" << *kdtree); 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /graph_core/tests/src/path_post_processing_test.cpp: -------------------------------------------------------------------------------- 1 | // Graph core required headers 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | // Load the logger's configuration 11 | std::string path_to_config_folder = std::string(TEST_DIR); 12 | std::string logger_file = path_to_config_folder + "/logger_param.yaml"; 13 | 14 | if (argc > 1) 15 | logger_file = path_to_config_folder + "/" + std::string(argv[1]); // or take it from argument 16 | 17 | cnr_logger::TraceLoggerPtr logger = 18 | std::make_shared("path_post_processing_test", logger_file); 19 | 20 | // Load params 21 | std::string command = "cnr_param_server --path-to-file " + std::string(TEST_DIR) + "/graph_core_tests_params.yaml"; 22 | CNR_INFO(logger, "Executing command: " << command); 23 | int ret_code = std::system(command.c_str()); 24 | if (ret_code != 0) 25 | { 26 | CNR_ERROR(logger, "Error: command " << command << " failed"); 27 | return ret_code; 28 | } 29 | 30 | command = "cnr_param_server --path-to-file " + std::string(TEST_DIR) + "/path.yaml"; 31 | CNR_INFO(logger, "Executing command: " << command); 32 | ret_code = std::system(command.c_str()); 33 | if (ret_code != 0) 34 | { 35 | CNR_ERROR(logger, "Error: command " << command << " failed"); 36 | return ret_code; 37 | } 38 | 39 | command = "cnr_param_server --path-to-file " + std::string(TEST_DIR) + "/tree.yaml"; 40 | CNR_INFO(logger, "Executing command: " << command); 41 | ret_code = std::system(command.c_str()); 42 | if (ret_code != 0) 43 | { 44 | CNR_ERROR(logger, "Error: command " << command << " failed"); 45 | return ret_code; 46 | } 47 | 48 | // Define namespace for parameters retrieving 49 | std::string param_ns = "/graph_core_tests"; // must begin with "/" 50 | 51 | // Define the collision checker (foo collision checker) 52 | double min_cc_distance; 53 | double default_min_cc_distance = 0.01; 54 | graph::core::get_param(logger, param_ns, "min_cc_distance", min_cc_distance, 55 | min_cc_distance); // wrapper to cnr_param functions 56 | 57 | if (min_cc_distance <= 0) 58 | min_cc_distance = default_min_cc_distance; 59 | 60 | graph::core::CollisionCheckerPtr collision_checker = 61 | std::make_shared(logger, min_cc_distance); 62 | 63 | // Define a cost function (Euclidean metrics) 64 | graph::core::MetricsPtr metrics = std::make_shared(logger); 65 | 66 | // Load the path and tree computed in the first tutorial 67 | graph::core::PathPtr path; 68 | graph::core::get_param(logger, param_ns, "path", path, metrics, collision_checker); 69 | 70 | graph::core::TreePtr tree; 71 | graph::core::get_param(logger, param_ns, "tree", tree, metrics, collision_checker); 72 | 73 | // Set the tree as the tree associated to the path 74 | path->setTree(tree); 75 | 76 | CNR_INFO(logger, "Loaded path\n " << *path); 77 | CNR_INFO(logger, "Loaded tree\n " << *tree); 78 | 79 | // Do some example operations on the path, but to keep integrity of the original path with the tree 80 | // work on a clone of the path. The clone has the same waypoints but stored within different nodes instances 81 | // and does not have any tree associated. It shares the same metrics and collision checker with the original path. 82 | // However, you can clone also them if you want to work on different instances. We will not do it here. 83 | graph::core::PathPtr cloned_path = path->clone(); 84 | 85 | // Flip the path 86 | cloned_path->flip(); 87 | CNR_INFO(logger, "Flipped path\n " << *cloned_path); 88 | 89 | // Apply some post processing on the path (warp and simplify) 90 | graph::core::PathLocalOptimizerPtr path_opt = 91 | std::make_shared(collision_checker, metrics, logger); 92 | 93 | path_opt->setPath(cloned_path); 94 | 95 | double warp_min_conn_length; 96 | double default_warp_min_conn_length = 0.1; 97 | graph::core::get_param(logger, param_ns, "warp_min_conn_length", warp_min_conn_length, default_warp_min_conn_length); 98 | 99 | double warp_min_step_size; 100 | double default_warp_min_step_size = 0.05; 101 | graph::core::get_param(logger, param_ns, "warp_min_step_size", warp_min_step_size, default_warp_min_step_size); 102 | 103 | path_opt->warp(warp_min_conn_length, warp_min_step_size); 104 | CNR_INFO(logger, "Path after warp \n " << *cloned_path); 105 | 106 | double simplify_max_conn_length; 107 | double default_simplify_max_conn_length = 0.05; 108 | graph::core::get_param(logger, param_ns, "simplify_max_conn_length", simplify_max_conn_length, 109 | default_simplify_max_conn_length); 110 | 111 | path_opt->simplify(simplify_max_conn_length); 112 | CNR_INFO(logger, "Path after simplify \n " << *cloned_path); 113 | 114 | return 0; 115 | } 116 | -------------------------------------------------------------------------------- /graph_core/tests/src/run_all_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | std::string logger_file_ = "/logger_param_gtest.yaml"; 7 | 8 | void runBinary(const std::string& binary_path) 9 | { 10 | std::cout << "Running test binary: " << binary_path << " with logger file: " << logger_file_ << "..." << std::endl; 11 | 12 | std::string command = binary_path + " " + logger_file_; 13 | 14 | int result = std::system(command.c_str()); 15 | ASSERT_EQ(result, 0) << "Test binary " << binary_path << " failed with exit code " << result; 16 | } 17 | 18 | TEST(TestSuite, KdTreeTest) 19 | { 20 | runBinary("./kdtree_test"); 21 | } 22 | 23 | TEST(TestSuite, NodeConnectionTest) 24 | { 25 | runBinary("./node_connection_test"); 26 | } 27 | 28 | TEST(TestSuite, ComputePathTest) 29 | { 30 | runBinary("./compute_path_test"); 31 | } 32 | 33 | TEST(TestSuite, PathPostProcessingTest) 34 | { 35 | runBinary("./path_post_processing_test"); 36 | } 37 | 38 | int main(int argc, char** argv) 39 | { 40 | if (argc > 1) 41 | logger_file_ = std::string(argv[1]); 42 | 43 | ::testing::InitGoogleTest(&argc, argv); 44 | return RUN_ALL_TESTS(); 45 | } 46 | -------------------------------------------------------------------------------- /hooks/pre-commit-clang-format: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # clang-formatting 4 | echo "Executing clang-formatting pre-commit hook..." 5 | 6 | CLANG_FORMAT_FILE="graph_core/graph_core/.clang-format" 7 | 8 | if ! command -v clang-format &> /dev/null; then 9 | echo "Error: clang-format is not installed. Please install it and try again." 10 | exit 1 11 | fi 12 | 13 | if [ ! -f "$CLANG_FORMAT_FILE" ]; then 14 | echo "Error: .clang-format file not found at $CLANG_FORMAT_FILE" 15 | exit 1 16 | fi 17 | 18 | echo "Using clang-format configuration from: $(realpath "$CLANG_FORMAT_FILE")" 19 | 20 | for FILE in "$@"; do 21 | echo "Formatting $FILE" 22 | clang-format -i --style=file --assume-filename="$CLANG_FORMAT_FILE" "$FILE" 23 | git add "$FILE" 24 | done 25 | 26 | -------------------------------------------------------------------------------- /pull_request_template.md: -------------------------------------------------------------------------------- 1 | Fixes # 2 | 3 | ## Proposed Changes 4 | - 5 | - 6 | - 7 | --------------------------------------------------------------------------------