sampling-based-planners

C++ implementation of RRT, RRT*, and Informed-RRT* using kd-tree for searching NN and NBHD nodes. Supports arbitrary dimensions and compiles as a shared library.

Features

Provided as a shared library usable in C++14 or higher
You can execute at any dimensions without recompiling the shared library
To quickly search for NN and NBHD nodes, a node list consists of kd-tree.

Requirements

The following software packages are required for building the shared library:

A C++ compiler with C++14 or higher support
CMake 3.0 or higher
Eigen 3.0 or higher

If you would like to compile the example programs, add the following:

OpenCV 3.0 or higher

Build

The shared library (libplanner.so) can be build with following commands

$ git clone https://github.com/medalotte/sampling-based-planners.git
$ cd sampling-based-planners/lib
$ mkdir build && cd build
$ cmake ..
$ make

The example program can be run with following commands after build the shared library

$ cd <top of this repository>
$ git submodule update --init
$ cd examples/path-planning-2D
$ mkdir build && cd build
$ cmake ..
$ make

Usage

1. Include header file and set alias optionally

#include <planner.h>
namespace planner = pln

2. Define euclidean space

// difinition of two-dimensional space
const int DIM = 2;
pln::EuclideanSpace space(DIM);

// definition of bounds of each dimension
std::vector<pln::Bound> bounds{pln::Bound(0, 100.0),
                               pln::Bound(0, 100.0)};

// set bounds to space
space.setBound(bounds);

3. Define constraints

i. Point cloud type

// definition of obstacle (point cloud type)
std::vector<pln::PointCloudConstraint::Hypersphere> obstacles;
obstacles.emplace_back(pln::State(10.0, 20.0),  10.0);  // x : 10.0, y : 20.0, radius : 10.0
obstacles.emplace_back(pln::State(50.0, 70.0),  20.0);  // x : 50.0, y : 70.0, radius : 20.0
obstacles.emplace_back(pln::State(-10.0, 120.0), 30.0); // there is no probrem out of range

// definition of constraint using std::shared_ptr
auto constraint = std::make_shared<pln::PointCloudConstraint>(space, obstacles)

ii. Image type (use OpenCV for simplicity)

// read image
auto world = cv::imread("./example.png", CV_8UC1);

// definition of constraint array
std::vector<pln::ConstraintType> map(world.cols * world.rows, pln::ConstraintType::ENTAERABLE);

for(int yi = 0; yi < world.rows; yi++) {
    for(int xi = 0; xi < world.cols; xi++) {
        if(world.data[xi + yi * world.cols] != 255) {
            map[xi + yi * world.cols] = pln::ConstraintType::NOENTRY;
        }
    }
}

std::vector<uint32_t> each_dim_size{(uint32_t)world.cols, (uint32_t)world.rows};

// definition of constraint using std::shared_ptr
auto constraint = std::make_shared<pln::GridConstraint>(space, map, each_dim_size);

4. Solve

// definition of planner (you can set some parameters at optional argument)
// pln::RRT planner(DIM);
// pln::RRTStar planner(DIM);
pln::InformedRRTStar planner(DIM);

// set constraint
planner.setProblemDefinition(constraint);

// definition of start and goal state
pln::State start(5.0, 5.0);
pln::State goal(90.0, 90.0);

// solve
bool status = planner.solve(start, goal);
if(status) {
    auto& result = planner.getResult();
    for(const auto& r : result) {
        std::cout << r << std::endl;
    }
}
else {
    std::cout << "Could not find path" << std::endl;
}