MRSL JPS3D Library
1.1
An implementaion of Jump Point Search on 3D voxel map
|
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.
Simply run following commands to install dependancy:
#### A) Simple cmake
#### B) Using CATKIN
Run following command in the build
folder for testing the executables:
If everything works, you should see the results as:
Note that in other repository, add following commands in CMakeLists.txt
in order to correctly link jps3d
:
Two libs will be installed: the standard jps_lib
and a variation dmp_lib
.
To start a simple JPS
planning thread:
First, the collision checking util must be loaded as:
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:
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*
:
Tow planners are provided for 2D and 3D map:
JPSPlanner2D
JPSPlanner3D
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.
An example in 3D map is presented in test/test_planner_3d.cpp
with the yaml data/simple3d.yaml
.
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.
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
.
For more details, please refer to Doxygen.