MoveIt!自定义运动规划算法的方法

       我们通常以Binary的方式在ROS中安装MoveIt!，但是这种方式的缺点是无法将自己写的运动算法（比如在OMPL中的算法基础上进行修改）集成到MoveIt!中，而以source的方式来安装，可以解决这个问题。本篇博客记录一下具体的安装步骤。

配置：Ubuntu 16.04，ROS Kinetic

主要参考资料如下：

Build MoveIt! From Source

Building Common MoveIt! Dependencies from Source in Catkin

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

Custom state sampler in MoveIt!

1、Uninstall moveit (sudo apt-get install方式安装的)

如果在前期使用ROS的过程中，已经安装了moveit，首先是将其卸载，卸载方法如下：

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

或者是

sudo apt-get purge ros-kinetic-moveit-*
sudo apt-get autoremove

2、安装moveit！from Source

2.1 安装前预配置

首先，安装ROS，运行如下命令，检查是否安装了最新版本的packages：

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

源代码安装需要wstool, catkin_tools, clang-format，其中clang-format是可选的：

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

建立单独的工作空间，最好不要和catkin_ws混在一起，会很乱。

mkdir ~/ws_moveit
cd ~/ws_moveit

然后，source  ROS workspace 来加载必要的环境变量，这个操作将会加载 ${ROS_DISTRO}，后面有用。这个取决于现在所用的ROS版本，我们现在用的是ROS Kinetic，所以：

source /opt/ros/kinetic/setup.bash

2.2 下载并Build MoveIt!

默认地，是在最新的程序分支上build，现在系统默认地是用 melodic-devel 作为主分支，此分支可以builds for ROS Kinetic and newer, e.g. Ubuntu 16.04 and newer。

wstool init src
wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/melodic-devel/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
catkin build

然后，Setup your environment - you can do this every time you work with this particular source install of the code, or you can add this to your .bashrc (不建议，会和catkin_ws冲突)：

source ~/ws_moveit/devel/setup.bash # or .zsh, depending on your shell

3、安装OMPL

Building Common MoveIt! Dependencies from Source in Catkin中介绍了怎么从source源代码安装MongoDB, OMPL, or FCL ，本文暂时只介绍OMPL的安装方法。

OMPL is MoveIt!’s main planning library, 下载 OMPL repos from either Bitbucket or Github放在ws_moveit/src里面。我下载的是github上的kinetic devel分支源代码。然后手动将下面的 package.xml 下载到src/ompl中，which is used in the ROS release wrapper for OMPL。

cd src/ompl
wget https://raw.githubusercontent.com/ros-gbp/ompl-release/debian/kinetic/xenial/ompl/package.xml

下面这一步是Building Common MoveIt! Dependencies from Source in Catkin中介绍的，但是经过测试，此步不需要，而且会导致编译不成功，暂时先记录下来：编辑moveit/moveit_planners/ompl/CMakeLists.txt，然后调换 ${OMPL_INCLUDE_DIRS} 和 ${catkin_INCLUDE_DIRS}的前后顺序。

${catkin_INCLUDE_DIRS}
${OMPL_INCLUDE_DIRS} 

changed to

${OMPL_INCLUDE_DIRS} 
${catkin_INCLUDE_DIRS}

 

然后，修改安装的目标文件夹。打开../ompl/src/ompl/CMakeList.txt，将第83行的${CMAKE_INSTALL_LIBDIR} 修改为 “/opt/ros/kinetic/lib/x86_64_linux_gnu” 或者 "/opt/ros/kinetic/lib/"，总有一款适合你。Custom state sampler in MoveIt!

cd ../ompl/src/ompl/CMakeList.txt
${CMAKE_INSTALL_LIBDIR}

changed to

"/opt/ros/kinetic/lib/x86_64_linux_gnu"
or
"/opt/ros/kinetic/lib/"

最后，编译，正常情况下，是可以编译成功的，注意是sudo catkin build：

cd ~/ws_moveit

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

（此步可选）

sudo catkin build

因为安装到/opt/ros/kinetic中，所以需要sudo权限

4、关于catkin_ws和ws_moveit共存的问题

现在系统里一共有两个自建的ROS工作空间：

catkin_ws：用catkin_make编译，放一些自己写的packages

ws_moveit：用catkin build编译，放moveit相关的packages

注意，catkin_make和catkin build不能在同一个工作空间下混用。

我的目标是用catkin_ws里面的包去调用ws_moveit里面的moveit，所以，通常按照以下步骤操作，注意：

1、必须始终在一个terminal中

2、不要将source ~/ws_moveit/devel/setup.bash 放在 .bashrc文件中

cd ~/ws_moveit
catkin config --extend /opt/ros/${ROS_DISTRO} --cmake-args -DCMAKE_BUILD_TYPE=Release
sudo catkin build
source ~/ws_moveit/devel/setup.bash

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

完成以上操作后，应该可以正常调用moveit了，运行以下命令来测试一下：

roslaunch ur5_moveit_config demo.launch

5 记一次失败的安装过程

最近电脑重装系统，重新源码安装moveit，但是一直失败。其实，一开始我也没发现安装是失败的，但是在运行如下命令：

roslaunch ur_gazebo ur5.launch
 
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
 
roslaunch ur5_moveit_config moveit_rviz.launch config:=true

一直报错：找不到moveit_simple_controller_manager，运行rospack list发现根本就没安装这个包，进而发现moveit的安装包路径竟然位于/opt/ros/kinetic/share中，不是自定义的路径如/home/xh/ws_moveit/src/...中，于是来回重装多遍，一直无法不成功。回想安装过程，并没有报错，而且也source了setup.bash文件。

后来无意中发现，我的catkin_ws和ws_moveit中有关于panda机器人的重合的安装包，导致两个工作空间无法共容，果断删了catkin_ws中的panda包，再次编译，source，运行echo $ROS_PACKAGE_PATH，获得如下结果，二者共存。图中路径不全，仅作参考。

 

6 自定义运动规划算法

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），并重命名为RRTstarxh.h和RRTstarxh.cpp，保存路径与原文件相同。

2、打开并修改RRTstarxh.h和RRTstarxh.cpp，将二者中的RRTstar替换为RRTstarxh

 

3、打开并修改planning_context_manager.cpp（~/ws_moveit/src/moveit/moveit_planners/ompl/ompl_interface/src）

      include <>应需要包含完整的头文件路径，并仿照别的算法，定义registerPlannerAllocator()。

4、打开并修改ompl_planning.yaml（~/catkin_ws/src/universal_robot/ur5_moveit_config/config），仿照RRTstar算法，添加RRTstarxh算法的配置，RRTstarxhkConfigDefault。

5、然后按照前文所述的方式编译，运行 roslaunch ur5_moveit_config demo.launch 测试是否成功添加，完成！