path_optimizer 移动到这里了
根据参考路径（点）生成可行路径。
catkin build path_optimizer
roslaunch path_optimizer demo.launch
如果想重新选择，在使用Publish Point工具的情况下任意位置双击即可。
在目前设置中不强制完全到达终点。
rosrun path_optimizer path_optimizer_benchmark
- Take inputs (red dots):
- (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.
- Smooth the reference path using IPOPT (yellow curve).
- Represent the path planning problem as a QP and solve it using OSQP.
