记录MoveIt自定义算法实现过程：moveit+ompl+自定义规划算法测试

1.源码安装moveit

1.1删除通过binary安装的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

1.5加载环境变量

source /opt/ros/melodic/setup.bash

1.6下载moveit源码并编译

wstool init src
wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall
wstool update -t src   #（下载包失败，按照以下链接去下载source）
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

所需要的包：https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall
下载melodic版本的源码到/home/xxx/ws_moveit/src
出现错误：

fzl@ubuntu:~/ws_moveit$ rosdep install -y --from-paths src --ignore-src --rosdistro melodic
ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
moveit_core: Cannot locate rosdep definition for [code_coverage]
moveit_resources_prbt_support: Cannot locate rosdep definition for [canopen_motor_node]
panda_moveit_config: Cannot locate rosdep definition for [franka_description]
pilz_industrial_motion_planner: Cannot locate rosdep definition for [code_coverage]

运行以下命令可以修复问题：

rosdep update --include-eol-distros

再重新执行即可

rosdep install -y --from-paths src --ignore-src --rosdistro melodic

2.源码安装ompl

2.1卸载OMPL
首先进入根目录的库文件，检查是否有OMPL库。

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

如果没有直接跳到下一步，如果有应该卸载。

$ sudo apt-get purge ros-melodic-ompl

2.2下载ompl

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

注意：git clone可能不好用，建议点击链接进行手动下载
先进入/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

2.3编译ompl

cd ~/ws_moveit/
sudo catkin make
source ~/ws_moveit/devel/setup.bash

编译失败，且单独编译ompl失败

CMake Error at CMakeModules/CompilerSettings.cmake:7 (add_compile_definitions): Unknown CMake command "add_compile_definitions".

重新按照以下步骤编译和安装ompl：
下载和更改CMakeList.txt文件文件后，然后在~/ws_moveit/src/ompl/文件夹下建文件“build/Release”

cd build/Release
cmake -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic ../..
#编译失败，且单独编译ompl失败
CMake Error at CMakeModules/CompilerSettings.cmake:7 (add_compile_definitions): Unknown CMake command "add_compile_definitions".
#解决方案：将/home/fzl/ws_moveit/src/ompl/CompilerSettings.cmake中的下面这行内容屏蔽后重新编译
#add_compile_definitions(_HAS_AUTO_PTR_ETC=0)
#然后重新编译
cmake -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic ../..

2.4安装ompl：

make -j4
sudo make install

之后再重新编译moveit即可（理论上在编译moveit之前应该在moveit中修改了ompl算法，编译之后即可使用。。。）

cd ~/ws_moveit/
sudo catkin make
#出现报错c++: internal compiler error: Killed (program cc1plus)，原因大概率是因为编译时内存不足
#尝试运行sudo catkin make -j1，编译ok
或者 sudo catkin make -j1

source ~/ws_moveit/devel/setup.bash

3.自定义规划算法，编译测试

3.1自定义算法配置：
新增myRRT.h和myRRT.cpp算法内容，另外修改配置文件，具体如下：
1、新增myRRT.h和myRRT.cpp；
2、～ws_moveit/src/moveit/moveit_planners/ompl/ompl_interface/src中找到planning_context_manager.cpp,添加头文件；
3、修改/home/arl/ws_moveit/src/panda_moveit_config/config/ompl_planning.yaml文件。

执行sudo catkin build出现问问题：
/home/fzl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.11: undefined reference to `ompl::geometric::myRRT::~myRRT()'
/home/fzl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.11: undefined reference to `ompl::geometric::myRRT::myRRT(std::shared_ptr<ompl::base::SpaceInformation> const&, bool)'
collect2: error: ld returned 1 exit status

解决方法：修改ompl的CMakeList.txt文件

打开~/ws_moveit/src/ompl/src/ompl/CMakeList.txt中约第80行的安装地址：DESTINATION ${CMAKE_INSTALL_LIBDIR}
将其改为：DESTINATION "/opt/ros/melodic/lib"，然后可以编译安装ompl
    #DESTINATION ${CMAKE_INSTALL_LIBDIR}
    DESTINATION "/opt/ros/melodic/lib"
    DESTINATION "/opt/ros/melodic/lib/x86_64-linux-gnu"

3.1自定义算法测试：采用myRRT规划路径

[ INFO] [1689340852.682664699]: Planner configuration 'panda_arm[myRRT]' will use planner 'geometric::myRRT'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1689340852.683686140]: panda_arm[myRRT]: Starting planning with 1 states already in datastructure
[ INFO] [1689340852.684248563]: panda_arm[myRRT]: Starting planning with 1 states already in datastructure
[ INFO] [1689340852.684516873]: panda_arm[myRRT]: Starting planning with 1 states already in datastructure
[ INFO] [1689340852.684877388]: panda_arm[myRRT]: Starting planning with 1 states already in datastructure
[ INFO] [1689340852.722074289]: panda_arm[myRRT]: Created 17 states
[ INFO] [1689340852.723003327]: panda_arm[myRRT]: Created 17 states
[ INFO] [1689340852.725208916]: panda_arm[myRRT]: Created 17 states
[ INFO] [1689340852.728204237]: panda_arm[myRRT]: Created 19 states
[ INFO] [1689340852.728476748]: ParallelPlan::solve(): Solution found by one or more threads in 0.045255 seconds

至此，在ros melodic上安装编译moveit、opml以及自定义规划算法测试完成。