在ompl中自定义运动规划算法

在上一篇中介绍了使用源码安装moveit和ompl的方法，接下来就是在ompl中写自己的算法并且在moveit中使用。

1.模仿RRTstar的文件RRTstar.h（~/ws_moveit/src/ompl/src/ompl/geomatric/planner/rrt）和RRTstar.cpp（~/ws_moveit/src/ompl/src/ompl/geomatric/planner/rrt/src），在相同位置下写自己的头文件和cpp文件。

2.修改planning_context_manager.cpp（~/ws_moveit/src/moveit/moveit_planners/ompl/ompl_interface/src），模仿着写就好，自己写的算法#include尽量使用完整的头文件路径，其他的就模仿写一份。

3.打开并修改ompl_planning.yaml（~/catkin_ws/src/universal_robot/ur5_moveit_config/config），同样是模仿着写就好。

把这两个文件改好了，就可以重新编译，然后打开demo就可以看到自己写的算法在context下的ompl中，就可以选择自己的算法。