Ubuntu1804下的Melodic版本Moveit和OMPL的源码安装，并自定义规划算法在Moveit上使用

1.Moveit的源码安装

因为binary安装的Moveit无法自定义算法，所以要把之前安装的先卸载。如果没有安装可以跳过1.1步。

1.1卸载二进制安装的Moveit

$ sudo apt-get remove ros-melodic-moveit-*

1.2源码安装Moveit

更新软件包

$ rosdep update
$ sudo apt-get update
$ sudo apt-get dist-upgrade

安装依赖

$ sudo apt-get install python-wstool python-catkin-tools clang-format-3.9 

创建Moveit工作空间

$ mkdir ~/ws_moveit
$ cd ~/ws_moveit

加载环境变量

$ source /opt/ros/melodic/setup.bash

下载moveit源码并编译

$ wstool init src
$ wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall
$ wstool update -t src
$ rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO}
$ catkin config --extend /opt/ros/${ROS_DISTRO} --cmake-args -DCMAKE_BUILD_TYPE=Release
$ sudo catkin build

没有报错就成功，如果有包丢失，或者安装失败需要重复语句，直到安装成功。
这里参考了road_of_god博客的第一章，(链接: link)。

2.OMPL源码安装

和第一部分一样，如果之前二进制安装了Moveit，会自动给你安装OMPL的，刚刚卸载了Moveit，但是OMPL还在里面，所以还要先卸载，如果没有安装过，可以跳过2.1

2.1卸载OMPL

参考洱棟七的知乎回答link
首先进入根目录的库文件，检查是否有OMPL库。

$ cd /opt/ros/melodic
$ find ./ -name "ompl*"
$ find ./ -name "libompl"

看看自己下面有没有这些包和库文件，如果没有直接跳到下一步，如果有应该卸载。

$ sudo apt-get purge ros-melodic-ompl

然后再运行前面的两条find命令，应该找不到ompl相关的文件了。
至此，卸载成功。

2.2源码安装OMPL

这里参考link的第三章。
首先下载ompl的源码

$ cd ~/ws_moveit/src
$ git clone https://github.com/ompl/ompl

如果clone不好用，可以直接进入连接，下载压缩包，再解压到ws_moveit/src。
刚刚我们在一个./lib目录下卸载了三个.so文件，所以要把新下载的文件再安装上去。
先进入/ompl/src/ompl看是否有一个CMakeList.txt文件，如果有跳过这一段话，如果没有要先下载：

$ cd ~/ws_moveit/src/ompl/src/ompl
$ wget https://raw.githubusercontent.com/ros-gbp/ompl-release/debian/melodic/xenial/ompl/package.xml

然后进行安装之前，所以要修改安装目录，打开~/ws_moveit/src/ompl/src/ompl/CMakeList.txt中约第80行的安装地址：

DESTINATION ${CMAKE_INSTALL_LIBDIR}

改为：

DESTINATION "/opt/ros/melodic/lib"

然后可以编译安装ompl

$ cd ~/ws_moveit
$ sudo catkin build

正常安装且没有报错后，进行2.1步的find命令会找到新安装的ompl库。
至此。ompl的源码成功。可以进入第三章的自定义规划算法，

下面是我遇到的问题，可以跳过

在自定义ompl的源码后，每次修改并且build之后，运行ros更换成自己的规划算法MyRRT会报错，程序直接卡死。报错显示无法识别MyRRT。
其实每次编译之后，ompl会在/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/下再创建三四个.so的库文件，而moveit真正运行ompl库其实就是这几个库文件，而不是/opt/ros/melodic/lib下的库文件，这个下面的库文件是和编译有关的，所以要把CmakeList.txt中修改的路径再修改回来。再改回来

DESTINATION ${CMAKE_INSTALL_LIBDIR}

修改planners下的规划程序，再次build后，运行你的程序就是你修改的了。
测试验证：进入～/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/src
找到MyRRT.c修改大概227行位置

OMPL_INFORM("%s: MyRRT Created %u states", getName().c_str(), nn_->size());

在更换为MyRRT作为路径规划的程序后，在终端打印的字符就变成了，MyRRT Created，证明修改成功。

3.自定义规划算法

3.1自定义RRT

参考连接link
这个连接其中还有很多ompl的总结，可参考。
以ompl自带的RRT算法为例子：
第一步 进入～/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/src，把RRT.c和RRT.h复制重命名为MyRRT.c和MyRRT.h，进入这两个复制的文件，把里面所有的RRT字符替换为MyRRT。
第二步 进入～moveit_ws/src/moveit/moveit_planners/ompl/ompl_interface/src中找到planning_context_manager.cpp,添加头文件

#include </home/ununtu/ws_moveit/src/ompl/src/ompl//geometric/planners/rrt/MyRRT.h>

不要直接复制，看看你自己的h文件目录在哪。
还是在planning_context_manager.cpp下找到这个函数

void ompl_interface::PlanningContextManager::registerDefaultPlanners()

在里面仿照其他的语句，添加

registerPlannerAllocator(  //
      "geometric::MyRRT",      //
      std::bind(&allocatePlanner<og::MyRRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

其实就是把RRT那条复制来，再把RRT改为MyRRT就可以保存了。
第三步 打开你要运行的程序的ompl_planning.yaml，moveit默认的路径在/home/arl/ws_moveit/src/panda_moveit_config/config/ompl_planning.yaml，但是你的包可能用的不是这个ompl_planning.yaml，找到你的这个yaml文件后，仿照着RRT添加说明：

MyRRT:
    type: geometric::MyRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05

参考RRT，文件下面还要添加

 - MyRRT

至此规划算法的修改完成。
如果有报错，参考连接 link

Errors     << moveit_planners_ompl:make /home/arl/ws_moveit/logs/moveit_planners_ompl/build.make.020.log                                                                                                   
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::~TORRT()’未定义的引用
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::TORRT(std::shared_ptr<ompl::base::SpaceInformation> const&)’未定义的引用
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/moveit_planners_ompl/moveit_ompl_planner] Error 1
make[1]: *** [ompl_interface/CMakeFiles/moveit_ompl_planner.dir/all] Error 2
make[1]: *** 正在等待未完成的任务....
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::~TORRT()’未定义的引用
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::TORRT(std::shared_ptr<ompl::base::SpaceInformation> const&)’未定义的引用
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/moveit_planners_ompl/generate_state_database] Error 1
make[1]: *** [ompl_interface/CMakeFiles/moveit_generate_state_database.dir/all] Error 2
make: *** [all] Error 2
cd /home/arl/ws_moveit/build/moveit_planners_ompl; catkin build --get-env moveit_planners_ompl | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

显示的是库文件无法识别刚修改的MyRRT，解决办法：

$ cd /home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/
$ sudo rm -rf libmoveit_ompl_interface.so.*  //把lib下生成的库文件先删除
$ cd ~/ws_moveit/
$ sudo catkin build   //重新编译

还不行的话，按照第二章的方法，卸载重装OMPL。

3.2编译测试

$ cd ~/ws_moveit/
$ sudo catkin build

如果没有报错说明修改成功了。
在运行你的launh文件，打开Rviz和MotionPlanning组件，设置好目标点后选择你的MyRRT算法，并开始planning，终端打印的规划的信息，Rviz显示规划的演示，说明修改成功。
测试验证：进入～/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/src
找到MyRRT.c修改大概227行位置

OMPL_INFORM("%s: MyRRT Created %u states", getName().c_str(), nn_->size());

在更换为MyRRT作为路径规划的程序后，在终端打印的字符就变成了，MyRRT Created，证明修改成功。
之后可以写你自己的规划算法并按照上面方法添加到moveit上面。