An implementation of k-PRM path planner with corridor-based trajectory optimization

This is course project of Group 17 for RO47005 Planning and Decision Making in TU Delft. We implemented k-PRM for front-end path searching and an iterative method for back-end corridor-based trajectory optimization.

1. Our contribution

We implement occupancy map generation from scratch (please check src/planner/map_server/).
We implement PRM planner and k-d tree from scratch .
We implement A-star method for graph search from scratch (please check src/planner/prm_planner/).
We implement classic minimum-snap trajectory optimization (both optimization solution and closed-form solution) from scratch (please check src/planner/traj_opt/).
We implement corridor-based trajectory optimization from scratch (please check src/planner/traj_opt/). We use OSQP_Interface to solve QP problem. We also use Root-Finder to find tangent point of polynomial trajectory.
The simulation environment (including simulator and controller) we used is cloned from Fast-Planner
We use DecompROS for safe flight corridor generation directly from occupancy map.
We use mockamap with tiny modification to randomly generate point clouds and publish only once.
We compared our PRM planner with RRTs

2. Build a demo

Step 1. Install Armadillo, which is required by uav_simulator package.

sudo apt-get install libarmadillo-dev

Step 2. Install OSQP, which is required by traj_opt package.

Step 3. clone this repo

cd ~/<your_catkin_ws>/src
git clone https://github.com/edmundwsy/RO47005-PDM-Final.git
cd ..

Step 4. Build the project

catkin_make

3. Run a simple demo

Load configuration files

source devel/setup.bash

3.1 Demo in random forest environment

Run following command to launch our project in rviz

roslaunch planner simulation.launch

Use 2D Nav Goal in rviz to publish a goal point

3.2 Demo in 3D Maze environment

Run following command to launch our project in rviz

roslaunch planner maze3d.launch

3.3 Compare with RRT

Run following command to launch rrt_planner

roslaunch rrt_planner simulation_with_map.launch

Structure

├── ./planner
│   ├── ./planner/prm_planner   <our implementation>
│   ├── ./planner/map_server    <our implementation>
│   ├── ./planner/rrt_planner
│   ├── ./planner/traj_opt      <our implementation>
│   └── ./planner/Utils
└── ./uav_simulator
    ├── ./uav_simulator/fake_drone
    ├── ./uav_simulator/local_sensing
    ├── ./uav_simulator/map_generator
    ├── ./uav_simulator/mockamap
    ├── ./uav_simulator/so3_control
    ├── ./uav_simulator/so3_quadrotor_simulator
    └── ./uav_simulator/Utils

License

Distributed under the MIT License. See LICENSE for more information.

Contact

Siyuan Wu - S.Wu-14@student.tudelft.nl Moji Shi - M.Shi-5@student.tudelft.nl Ranbao Deng - R.Deng@student.tudelft.nl Liangchen Sui - L.Sui@student.tudelft.nl

siyuanwu99/RO47005-PDM-Final