MRSL Jump Point Search Planning Library v1.1
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". 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".
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:
$ sudo apt update
$ sudo apt install -y libeigen3-dev libyaml-cpp-dev libboost-dev cmake
A) Simple cmake
$ mkdir build && cd build && cmake .. && make -j4
B) Using CATKIN
$ 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:
$ make test
If everything works, you should see the results as:
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
:
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:
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:
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*
:
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
,
in which we plan from start to goal using both A*
and JPS
.
The results are plotted in 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.
$ ./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
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
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, 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
.
$ ./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.