根据参考路径（点）生成可行路径。

catkin build path_optimizer
roslaunch path_optimizer demo.launch

如果想重新选择，在使用Publish Point工具的情况下任意位置双击即可。

在目前设置中不强制完全到达终点。

rosrun path_optimizer path_optimizer_benchmark

1. Take inputs (red dots):
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.
3. Smooth the reference path using IPOPT (yellow curve).
   
4. Represent the path planning problem as a QP and solve it using OSQP.