https://github.com/kumarrobotics/jps3d
A C++ implementation of Jump Point Search on both 2D and 3D maps
https://github.com/kumarrobotics/jps3d
Last synced: 10 months ago
JSON representation
A C++ implementation of Jump Point Search on both 2D and 3D maps
- Host: GitHub
- URL: https://github.com/kumarrobotics/jps3d
- Owner: KumarRobotics
- License: bsd-3-clause
- Created: 2017-01-16T00:11:09.000Z (about 9 years ago)
- Default Branch: master
- Last Pushed: 2021-02-18T22:02:04.000Z (almost 5 years ago)
- Last Synced: 2025-03-29T23:08:10.426Z (11 months ago)
- Language: C++
- Homepage:
- Size: 1.87 MB
- Stars: 404
- Watchers: 43
- Forks: 120
- Open Issues: 7
-
Metadata Files:
- Readme: README.md
- License: LICENSE
Awesome Lists containing this project
README
# MRSL Jump Point Search Planning Library v1.1
[](https://app.wercker.com/project/byKey/880ab5feaff25f0483e5f2c4f834b8c0)
- - -
Jump Point Search for path planning in both 2D and 3D environments. Original jump point seach algorithm is proposed in ["D. Harabor and A. Grastien. Online Graph Pruning for Pathfinding on Grid Maps. In National Conference on Artificial Intelligence (AAAI), 2011"](https://www.aaai.org/ocs/index.php/AAAI/AAAI11/paper/download/3761/4007). The 3D version is proposed in ["S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor and V. Kumar. Planning Dynamically Feasible Trajectories for Quadrotors using Safe Flight Corridors in 3-D Complex Environments. ICRA 2017"](http://ieeexplore.ieee.org/abstract/document/7839930/).
The distance map planner (`DMPlanner`) is also included in this repo. `DMPlanner` inflate a artificial potential field around obstacles and plan a safer path within a certain region. More detials are refered in the latter section.
## Installation
#### Required:
- Eigen3
- yaml-cpp
Simply run following commands to install dependancy:
```bash
$ sudo apt update
$ sudo apt install -y libeigen3-dev libyaml-cpp-dev libboost-dev cmake
```
#### A) Simple cmake
```bash
$ mkdir build && cd build && cmake .. && make -j4
```
#### B) Using CATKIN
```bash
$ mv jps3d ~/catkin_ws/src
$ cd ~/catkin_ws & catkin_make_isolated -DCMAKE_BUILD_TYPE=Release
```
#### CTest
Run following command in the `build` folder for testing the executables:
```bash
$ make test
```
If everything works, you should see the results as:
```bash
Running tests...
Test project /home/sikang/thesis_ws/src/packages/jps3d/build
Start 1: test_planner_2d
1/3 Test #1: test_planner_2d .................. Passed 0.95 sec
Start 2: test_planner_3d
2/3 Test #2: test_planner_3d .................. Passed 0.00 sec
Start 3: test_distance_map_planner_2d
3/3 Test #3: test_distance_map_planner_2d ..... Passed 1.26 sec
100% tests passed, 0 tests failed out of 3
Total Test time (real) = 2.22 sec
```
#### Include in other projects
Note that in other repository, add following commands in `CMakeLists.txt` in order to correctly link `jps3d`:
```sh
find_package(jps3d REQUIRED)
include_directories(${JPS3D_INCLUDE_DIRS})
...
add_executable(test_xxx src/test_xxx.cpp)
target_link_libraries(test_xxx ${JPS3D_LIBRARIES})
```
Two libs will be installed: the standard `jps_lib` and a variation `dmp_lib`.
## JPS Usage
To start a simple `JPS` planning thread:
```c++
JPSPlanner2D planner(false); // Declare a 2D planner
planner.setMapUtil(map_util); // Set collision checking function
planner.updateMap(); // Set map, must be called before plan
bool valid = planner.plan(start, goal, 1); // Plan from start to goal with heuristic weight 1, using JPS
```
First, the collision checking util must be loaded as:
```c++
planner.setMapUtil(MAP_UTIL_PTR); // Set collision checking function
```
The `MAP_UTIL_PTR` can be either `JPS::OCCMapUtil` for 2D or `JPS::VoxelMapUtil` for 3D. It can be confusing to set up this util, see the example code for more details.
Second, call the function `updateMap()` to allocate the internal map:
```
planner.updateMap(); // Set map, must be called before plan
```
Finally, call the function `plan` to plan a path from `start` to `goal`. The third input is the heuristic weight, the forth input indicates whether planning with `JPS` or `A*`.
By default, the forth input is set to be `true`, means the `JPS` is the default back-end. To use normal `A*`:
```c++
bool valid_astar = planner.plan(start, goal, 1, false); // Plan from start to goal with heuristic weight 1, using A*
```
Tow planners are provided for 2D and 3D map:
- ```JPSPlanner2D```
- ```JPSPlanner3D```
#### Planning Example
An example code for a 2D map is given in [`test/test_planner_2d.cpp`](https://github.com/sikang/jps3d/blob/master/test/test_planner_2d.cpp),
in which we plan from start to goal using both ```A*``` and ```JPS```.
The results are plotted in [corridor.png](https://github.com/sikang/jps3d/blob/master/data/corridor.png).
Green path is from ```A*```, red path is from ```JPS```. Even though they are using two different routes and `JPS` is much faster, the distance/cost of two paths is the same.
In other words, `JPS` guarantees the optimality but saves a significant amount of computation time.
```bash
$ ./build/test_planner_2d ./data/corridor.yaml
start: 2.5 -2
goal: 35 2.5
origin: 0 -5
dim: 799 199
resolution: 0.05
JPS Planner takes: 5.000000 ms
JPS Path Distance: 35.109545
JPS Planner takes: 5.000000 ms
AStar Planner takes: 62.000000 ms
AStar Path Distance: 35.109545
```
An example in 3D map is presented in [`test/test_planner_3d.cpp`](https://github.com/sikang/jps3d/blob/master/test/test_planner_3d.cpp) with the yaml `data/simple3d.yaml`.
##### Mapping Example
To generate map in `yaml` format which can be loaded directly in the test node, a simple executable file [`test/create_map.cpp`](https://github.com/sikang/jps3d/blob/master/test/create_map.cpp) is used.
User can easily change the location of blocks in the source code.
## DMP Usage
`DMPlanner` stands for distance map planner which utilizes the artificial potential field to find a safer local path around a given path. We changed the API for setting map of `DMPlanner` in v1.1.
The key feature of this planner is its ability to push the path away from obstacles as much as possible. An example is given in the following figure
[example_dmp.png](https://github.com/sikang/jps3d/blob/master/data/example_dmp.png), where red path comes from `JPS` which is always attached to obstacles and blue path is derived from `DMP` which is much safer.
The code for generating this figure is given in [`test/test_distance_map_2d.cpp`](https://github.com/sikang/jps3d/blob/master/test/test_distance_map_planner_2d.cpp).
```bash
$ ./build/test_distance_map_planner_2d ./data/corridor.yaml
start: 2.5 -2
goal: 35 2.5
origin: 0 -5
dim: 799 199
resolution: 0.05
JPS Planner takes: 7.000000 ms
JPS Path Distance: 35.109545
DMP Planner takes: 8.000000 ms
DMP Path Distance: 37.186501
```
## Doxygen
For more details, please refer to [Doxygen](https://kumarrobotics.github.io/jps3d).