/path_optimizer

Optimization-based real-time path planning for vehicles.

Primary LanguageC++MIT LicenseMIT

path_optimizer

🔔 CHECK OUT THE NEWER VERSION path_optimizer_2 and path_optimizer_ilqr


This ROS package generates feasible paths for non-holonomic vehicles.
2021-01-07 01-19-29屏幕截图.png

Simulation videos

simulation

Run demos

0. Install dependencies and build

  • ROS kinetic on Ubuntu 16.04
  • OpenCV 3
mkdir -p workspace/src && cd workspace/src
git clone git@github.com:LiJiangnanBit/path_optimizer.git
sudo bash path_optimizer/scripts/install_deps.sh
cd ..
catkin build path_optimizer
source devel/setup.bash

install_deps.sh will install other dependencies (Those already installed will be skipped). These dependencies include:

1. Demo

A png image is loaded as the grid map. You can click to specify the global reference path and the start/goal state of the vehicle.

roslaunch path_optimizer demo.launch

(1) Pick reference points using "Publish Point" tool in RViz.

  • Pick at least six points.
  • There are no hard and fast rules about the spacing of the points.
  • If you want to abandon the chosen points, just double click anywhere when using the "Publish Point" tool.
  • You can replace gridmap.png with other black and white images. Note that the resolution in demo.cpp is set to 0.2m, whick means that the length of one pixel is 0.2m on the map.
  • In application, the reference path is given by a global path or by a search algorithm like A*.

选点.gif

(2) Pick start state using "2D Pose Estimate" tool and pick goal state using "2D Nav Goal" tool.

  • Currently, it's not strictly required to reach the goal state. But this can be changed.
  • The start state must be ahead of the first reference point.

规划.gif

2. Benchmark test

This is a computation time test.

rosrun path_optimizer path_optimizer_benchmark

Usage

Refer to demo.cpp
The parameters that you can change can be found in planning_flags.cpp.

How it works

Refer here.

  1. Take inputs (red dots):
    2020-02-13 16-27-46屏幕截图.png
  2. (Optional) Use B spline curve fitting to make the path continuous and then search around it for a more reasonable reference path (yellow dots). This step can be skipped by changing settings.
    2020-02-13 16-27-56屏幕截图.png
  3. Smooth the reference path using IPOPT (yellow curve).
    2020-02-13 16-28-05屏幕截图.png
  4. Represent the path planning problem as a QP and solve it using OSQP.
    2020-02-13 16-28-19屏幕截图.png