Ubuntu18.04 ROS melodic版本环境下 Moveit!和OMPL的源码安装 以及 在Moveit!中运行自定义的路径规划算法

1.安装源码Moveit!

1.1卸载原本的Moveit!

要想安装源码Moveit!首先需要卸载ROS自带的Moveit!，在终端中输入如下指令：

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

1.2更新工作环境

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

1.3安装依赖

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

1.4安装源码Moveit!

首先，打开一个新终端，创建工作空间

mkdir ~/ws_moveit
cd ~/ws_moveit 
source ./devel/setup.bash

然后就是安装源码的Moveit!

$ wstool init src
$ wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall

我在执行第2行指令会报错。其实这一行的作用就是在后面的网址中下载相应的功能包，所以可以直接浏览器打开命令中的网址,里面的内容如下：

# This file is intended for users who want to build MoveIt from source.
# Used with wstool, users can download source of all packages of MoveIt.
- git:
    local-name: moveit_msgs
    uri: https://github.com/ros-planning/moveit_msgs.git
    version: master
- git:
    local-name: moveit_resources
    uri: https://github.com/ros-planning/moveit_resources.git
    version: master
- git:
    local-name: geometric_shapes
    uri: https://github.com/ros-planning/geometric_shapes.git
    version: noetic-devel
- git:
    local-name: srdfdom
    uri: https://github.com/ros-planning/srdfdom
    version: noetic-devel
- git:
    local-name: moveit
    uri: https://github.com/ros-planning/moveit.git
    version: master
- git:
    local-name: rviz_visual_tools
    uri: https://github.com/PickNikRobotics/rviz_visual_tools
    version: master
- git:
    local-name: moveit_visual_tools
    uri: https://github.com/ros-planning/moveit_visual_tools.git
    version: master
- git:
    local-name: moveit_tutorials
    uri: https://github.com/ros-planning/moveit_tutorials.git
    version: master
- git:
    local-name: panda_moveit_config
    uri: https://github.com/ros-planning/panda_moveit_config.git
    version: noetic-devel

进入每个功能包的网址下载压缩包，然后在工作空间下的src目录中解压即可达成和第二行代码一样的作用。
但一定要注意功能包的版本问题！！！
geometric_shapes要用neotic-devel，moveit_resources和moveit_msgs要用master，其他包用melodic-devel。
然后，按照官网给出的教程，在终端中执行如下指令(在ws_moveit目录下)

wstool update -t src
rosdep install -y --from-paths src --ignore-src --rosdistro melodic
catkin config --extend /opt/ros/melodic --cmake-args -DCMAKE_BUILD_TYPE=Release
sudo catkin build

但是，上述指令第一行可以不用执行！！！它的作用是更新刚刚下载的功能包的版本，但其实不需要更新。
我在执行第二行指令的时候报错如下：

rosdep找不到依赖：Cannot locate rosdep definition for [joy]

解决方法：

sudo apt-get install ros-melodic-joy

这里的[xxx]，就是报错[ ]里的。
这种报错有很多条，都按照最高操作即可解决。
有个要注意的点是：如果报错里的[]中是[xxx_zzz]，那么在执行需要把"_“变为”-"，即：sudo apt-get install ros-melodic-xxx-zzz
在执行完最后一行指令如果是如下所示，就成功了

[build] Summary: All 43 packages succeeded!                                    
[build]   Ignored:   7 packages were skipped or are blacklisted.               
[build]   Warnings:  None.                                                     
[build]   Abandoned: None.                                                     
[build]   Failed:    None.                                                     
[build] Runtime: 16.1 seconds total. 

如果失败，可能就是下载的软件包的版本不对，直接在文件中删除版本错误的功能包然后按照上述方法重新下载正确的版本，再执行上述指令即可。

2安装源码OMPL

2.1卸载原本的OMPL

sudo apt-get purge ros-melodic-ompl

2.2安装源码OMPL

cd ~/ws_moveit/src
git clone https://github.com/ompl/ompl
sudo catkin build

3自定义路径规划算法

为了快速检验安装是否正确，我直接在原来的RRTconnect算法基础上修改一下。

3.1复制文件

在ws_moveit/src/ompl/src/ompl/geometric/planners/rrt目录下，把RRTconnect.h文件复制并重命名为cqRRTconnect.h
同样，在ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/src目录下把RRTconnect.cpp文件复制并重命名为cqRRTconnect.cpp。
然后在这两个文件中把RRTconnect全部替换为cqRRTconnect
注意一定要把RRTconnect.h中的：

#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_
#define OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_

改成和这个不一样的，比如：

#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_cqRRT_CONNECT_
#define OMPL_GEOMETRIC_PLANNERS_RRT_cqRRT_CONNECT_

修改 planning_context_manager.cpp

在moveit_ws/src/moveit/moveit_planners/ompl/ompl_interface/src中的planning_context_manager.cpp中添加头文件：

#include <ompl/geometric/planners/rrt/cqRRTConnect.h>

然后找到下面的函数

void ompl_interface::PlanningContextManager::registerDefaultPlanners()

仿照添加：

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

修改 ompl_planning.yaml

在你的机械臂的功能包里找到ompl_planning.yaml。我是在UR机械臂上运行的，所以我的这个文件的地址是：~/ws_moveit/src/universal_robot/ur5_moveit_config/config(我把UR的包也放到了ws_moveit工作空间中，而上面安装的时候其实默认安装了panda的包，所以在ws_moveit/src/panda_moveit_config/config中也能找到
在ompl_planning.yaml中仿造RRTConnect，添加：

cqRRTConnect:
    type: geometric::cqRRTConnect
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

同样在下面仿造RRTConnect，添加：

 - cqRRTConnect

最后再执行：

cd 
cd ws_moveit
sudo catkin build

至此，完成自定义算法的添加。
启动机械臂仿真即可看到，自定义算法已经成功添加。