moveit自定义避障算法

一.moveit源码安装

1.卸载之前通过apt-get装的

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

2.更新软件包

rosdep update
sudo apt update
sudo apt dist-upgrade 

3.安装依赖

 sudo apt install python-wstool python-catkin-tools clang-format-10 python-rosdep 

4.新建工作空间

mkdir ~/ws_moveit
cd ~/ws_moveit 

5.加载环境变量（This will load the ${ROS_DISTRO} variable, needed for the next step.）

source /opt/ros/melodic/setup.bash

6.下载源代码

 如果第三步因为网络太慢下载不了，可以直接进入github，搜索对应第二步所需文件下载，放入src中。

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 

7.编译Moveit

catkin build
source ~/ws_moveit/devel/setup.bash 

二.ompl安装

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

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

三.自定义运动规划算法

1.在/home/hy/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/src/

    创建文件MyRRTConnect.cpp

    在/home/hy/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/

    创建文件MyRRTConnect.h

2.将上述两个文件中的RRTConnect都改成MyRRTConnect

3.在/home/hy/ws_moveit/src/moveit/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp中加入

#include </home/hy/ws_moveit/src/ompl/src/ompl/geometric/planners/rrt/MyRRTConnect.h>

void ompl_interface::PlanningContextManager::registerDefaultPlanners()
{
  registerPlannerAllocatorHelper<og::AnytimePathShortening>("geometric::AnytimePathShortening");
  registerPlannerAllocatorHelper<og::BFMT>("geometric::BFMT");
  registerPlannerAllocatorHelper<og::BiEST>("geometric::BiEST");
  registerPlannerAllocatorHelper<og::BiTRRT>("geometric::BiTRRT");
  registerPlannerAllocatorHelper<og::BKPIECE1>("geometric::BKPIECE");
  registerPlannerAllocatorHelper<og::EST>("geometric::EST");
  registerPlannerAllocatorHelper<og::FMT>("geometric::FMT");
  registerPlannerAllocatorHelper<og::KPIECE1>("geometric::KPIECE");
  registerPlannerAllocatorHelper<og::LazyPRM>("geometric::LazyPRM");
  registerPlannerAllocatorHelper<og::LazyPRMstar>("geometric::LazyPRMstar");
  registerPlannerAllocatorHelper<og::LazyRRT>("geometric::LazyRRT");
  registerPlannerAllocatorHelper<og::LBKPIECE1>("geometric::LBKPIECE");
  registerPlannerAllocatorHelper<og::LBTRRT>("geometric::LBTRRT");
  registerPlannerAllocatorHelper<og::PDST>("geometric::PDST");
  registerPlannerAllocatorHelper<og::PRM>("geometric::PRM");
  registerPlannerAllocatorHelper<og::PRMstar>("geometric::PRMstar");
  registerPlannerAllocatorHelper<og::ProjEST>("geometric::ProjEST");
  registerPlannerAllocatorHelper<og::RRT>("geometric::RRT");
  registerPlannerAllocatorHelper<og::RRTConnect>("geometric::RRTConnect");
  registerPlannerAllocatorHelper<og::MyRRTConnect>("geometric::MyRRTConnect");//hy
  registerPlannerAllocatorHelper<og::RRTstar>("geometric::RRTstar");
  registerPlannerAllocatorHelper<og::SBL>("geometric::SBL");
  registerPlannerAllocatorHelper<og::SPARS>("geometric::SPARS");
  registerPlannerAllocatorHelper<og::SPARStwo>("geometric::SPARStwo");
  registerPlannerAllocatorHelper<og::STRIDE>("geometric::STRIDE");
  registerPlannerAllocatorHelper<og::TRRT>("geometric::TRRT");
}

4.在/home/hy/aubo_ws/src/aubo_robot_1/aubo_i5_moveit_config/config/ompl_planning.yaml中加入

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

manipulator_i5:
  default_planner_config: RRTConnect
  planner_configs:
    - SBL
    - EST
    - LBKPIECE
    - BKPIECE
    - KPIECE
    - RRT
    - RRTConnect
    - MyRRTConnect
    - RRTstar
    - TRRT
    - PRM
    - PRMstar
    - FMT
    - BFMT
    - PDST
    - STRIDE
    - BiTRRT
    - LBTRRT
    - BiEST
    - ProjEST
    - LazyPRM
    - LazyPRMstar
    - SPARS
    - SPARStwo