• Stars
    star
    373
  • Rank 114,600 (Top 3 %)
  • Language
    C++
  • License
    BSD 3-Clause "New...
  • Created almost 8 years ago
  • Updated almost 4 years ago

Reviews

There are no reviews yet. Be the first to send feedback to the community and the maintainers!

Repository Details

A C++ implementation of Jump Point Search on both 2D and 3D maps

MRSL Jump Point Search Planning Library v1.1

wercker status


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.

Visualization

$ ./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.

Visualization

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.

More Repositories

1

msckf_vio

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
C++
1,722
star
2

kr_autonomous_flight

KR (KumarRobotics) autonomous flight system for GPS-denied quadrotors
C++
671
star
3

ublox

A driver for ublox gps
C++
446
star
4

sloam

Semantic LIDAR odometry and mapping for cylinderical objects (e.g. trees in forests)
C++
207
star
5

multicam_calibration

C++
118
star
6

conformal_lattice_planner

C++
116
star
7

kr_mav_control

Code for quadrotor control
C++
106
star
8

sdd_vio

Semi-Dense Direct Visual-Inertial Odometry
C++
97
star
9

mrsl_quadrotor

C++
93
star
10

motion_capture_system

Drivers for motion capture systems (Vicon and Qualisys, can be extended to compatible with other mocap systems)
C++
66
star
11

AllocNet

A lightweight learning-based trajectory optimization framework.
C++
58
star
12

bluefox2

ROS driver for the Matrix Vision mvBlueFOX cameras
C++
46
star
13

imu_vn_100

ROS driver for VN-100 of VectorNav Technologies
C
46
star
14

velodyne_puck

A simplified driver for Velodyne VLP16 (PUCK) based on the official ROS velodyne driver.
C++
37
star
15

gps-tools

ROS packages for use with GPS
C++
36
star
16

waypoint_navigation_plugin

C++
36
star
17

treescope

Python
35
star
18

spomp-system

CMake
35
star
19

kr_mp_design

A guidance for the design and evaluation of motion planners for quadrotors in Environments with Varying Complexities
35
star
20

imu_3dm_gx4

Driver for Lord Corporation Microstrain 3DM GX4 25
C++
32
star
21

flir_gige

A ros driver for flir ax5 gige thermal camera
C++
28
star
22

vicon

Code for working with the Vicon tracking system
C++
24
star
23

flea3

ROS driver for flea3/grasshopper3 camera
C++
21
star
24

MOCHA

Multi-robot Opportunistic Communication Framework for Heterogeneous Collaboration
Python
21
star
25

ouster_decoder

A low latency decoder for Ouster Lidars
C++
17
star
26

camera_base

Some base classes for simplifing ROS camera driver node.
C++
13
star
27

kr_param_map

A parameterized map generator for planning evaluations and benchmarking
C++
13
star
28

CoverageControl

Environment for coverage control and learning using GNN
C++
12
star
29

imu_3dm_gx3

Driver for Lord Corporation Microstrain 3DM GX3 25
C++
10
star
30

kr_ilqr_optimizer

Use iLQR package Altro to fly the quadrotor
C++
9
star
31

kr_opt_sfc

Optimal Convex Cover to Approximate Collision-free Space
9
star
32

asoom

C++
8
star
33

nanokontrol

C++
7
star
34

top_down_renderer

C++
5
star
35

grid_map

C++
4
star
36

spomp

C++
4
star
37

multi_mav_manager

This is a multi MAV manager which leverages mav_manager and quadrotor_control for each agent.
Shell
3
star
38

air_router

Python
3
star
39

video_magics

Shell
3
star
40

kr_utils

Collection of utils packages
C++
2
star
41

kr_docs

Repository for documentation relevant to the KumarRobotics organization
2
star
42

ublox-release

Release repo for ublox driver (ros1)
2
star
43

rangenet_inf

Python
1
star
44

kr_planning_msgs

C++
1
star
45

quadrotor_ukf

Quadrotor UKF
C++
1
star
46

kr_param_yaw

A Trajectory Optimization Method with Global Yaw Parameterization
1
star
47

semantics_manager

Python
1
star
48

starling2_interface

Starling 2 Integration
CMake
1
star