path_optimizer 移动到这里了

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

Demo 1：手动选择参考点

运行

catkin build path_optimizer
roslaunch path_optimizer demo.launch

使用Publish Point工具选择参考路径点

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

使用2D Pose Estimate和2D Nav Goal工具选择起点和终点

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

Benchmark效率测试

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.