在melodic中自定义路径规划算法（个人记录）

1.卸载二进制moveit

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

2.源码安装moveit

2.1更新软件包

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

2.2安装依赖

sudo apt-get install python-wstool python-catkin-tools clang-format-3.9 
sudo apt-get install python-rosdep(官网有这个命令MoveIt 1 Source Build - Linux | MoveIt)

2.3创建工作空间

mkdir ~/ws_moveit
cd ~/ws_moveit

2.4加载环境变量

source /opt/ros/melodic/setup.bash

2.5下载源码并编译（得会魔法）

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

3.ompl

3.1下载ompl源码

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

修改~/ws_moveit/src/ompl/src/ompl/CMakeList.txt中的

DESTINATION ${CMAKE_INSTALL_LIBDIR}为

DESTINATION "/opt/ros/melodic/lib/"

3.2进入编译(参考ompl上的方式https://github.com/ompl/ompl和ROS Kinetic版本下如何一步一步实现一个基本机械臂运动规划算法，如PRM？ - 知乎)

cd ~/ws_moveit/src/ompl

mkdir -p build/Release

cd build/Release
    cmake -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic ../..
    make -j4
    sudo make install

安装完后把3.1步中的路径再改回去（个人经验，我在后面的步骤中出现报错一直解决不了，后参考Ubuntu1804下的Melodic版本Moveit和OMPL的源码安装，并自定义规划算法在Moveit上使用_卸载moveit_不良高须的博客-CSDN博客改回去后解决了问题，提供一种思路，仅供参考）

问题：cmake可能无法识别某些指令，需升级cmake（参考Ubuntu升级Cmake的正确方式_cmake 升级_Liangtao`的博客-CSDN博客）

3.3编译ws_moveit

cd ~/ws_moveit

catkin config --extend /opt/ros/${ROS_DISTRO} --cmake-args -DCMAKE_BUILD_TYPE=Release

sudo catkin build(编译有警告，我没管，考虑可能是因为cmake版本更新的问题，因为更新前的那次编译没有警告)

4.测试源码安装的moveit能不能用

4.1 把自己的功能包放到~/ws_moveit/src里面

4.2编译

cd ~/ws_moveit

sudo catkin build

source ./devel/setup.bash

4.3启动功能包测试

roslaunch 你的包名 你的launch文件

5.模拟自定义路径规划算法

5.1创建新的算法（测试阶段，采用复制的方法）

在路径（～/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/src）复制RRT.cpp,并重命名为RRTcopy.cpp。在路径（～/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt）复制RRT.h,并重命名为RRTcopy.h

5.2查找替换

使用ctrl+F将文件中的RRT查找替换为RRTcopy

头文件中#ifndef和#define也需要改一下
#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_COPY_
#define OMPL_GEOMETRIC_PLANNERS_RRT_COPY_

5.3重新编译安装ompl

修改~/ws_moveit/src/ompl/src/ompl/CMakeList.txt中的

DESTINATION ${CMAKE_INSTALL_LIBDIR}为

DESTINATION "/opt/ros/melodic/lib/"

cd ~/ws_moveit/src/ompl/build/Release
    cmake -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic ../..
    make -j4
    sudo make install

安装完成后把DESTINATION改回去

5.4在moveit中添加算法

在路径（～ws_moveit/src/moveit/moveit_planners/ompl/ompl_interface/src）中找到planning_context_manager.cpp,添加自己的算法头文件，并注册自己的算法，如下图所示：

 5.5修改配置包的yaml文件

打开/home/arl/ws_moveit/src/你的moveit配置包/config/ompl_planning.yaml

在planner_configs:中添加RRTcopy

5.6编译测试

cd ～/ws_moveit/devel/.private/moveit_planners_ompl/lib/

sudo rm -rf libmoveit_ompl_interface.so*

sudo rm -rf libmoveit_ompl_planner_plugin.so*

cd ws_moveit

catkin config --extend /opt/ros/${ROS_DISTRO} --cmake-args -DCMAKE_BUILD_TYPE=Release

sudo catkin build

5.7启动launch文件看看

source ./devel/setup.bash

roslaunch 你的包名 你的launch文件

 成功！ 

参考：

MoveIt自定义路径规划算法（melodic版本）_road_of_god的博客-CSDN博客

OMPL学习--第三篇之源码安装Moveit!和OMPL(Melodic版本)_在moveit中集成自定义运动规划算法_OverCome-的博客-CSDN博客

MoveIt!自定义运动规划算法的方法_伍颜的博客-CSDN博客

ROS Kinetic版本下如何一步一步实现一个基本机械臂运动规划算法，如PRM？ - 知乎