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.
- 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.
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
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 ..
$ makeThe 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#include <planner.h>
namespace planner = pln// 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);// 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)// 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);// 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;
}Execute path planning on two-dimensional space
left: RRT, center: RRT*, right: Informed-RRT*
left: RRT, center: RRT*, right: Informed-RRT*
MIT