MoveIt自定义路径规划算法（melodic版本）

_Never_stop_

已于 2024-04-13 16:17:38 修改

阅读量6.4k

点赞数 8

分类专栏：机器人动起来文章标签： linux 人工智能自动驾驶 c++ 算法

于 2022-01-06 21:21:33 首次发布

本文链接：https://blog.csdn.net/road_of_god/article/details/122352259

版权

机器人动起来专栏收录该内容

2 篇文章 5 订阅

订阅专栏

moveit自定义路径规划算法，终于实现了，花了五天时间，一直解决不了bug，下面讲讲如何在moveit中自定义路径规划算法

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

常见bug：rosdep update会报错，更新不了，一定能解决的方法，见下连接：

rosdep update一定能通过de简单方法_努力是明天快乐的源泉-CSDN博客_rosdep update

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
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

常见bug：在进行 wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall报错：

ERROR in config: Unable to download URL [https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall]: <urlopen error [Errno 110] Connection timed out>

解决办法：

点击进入https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall

分别进入这６个链接，下载melodic版本的源码到/home/arl/ws_moveit/src

2.源码安装ompl

2.1下载ompl

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

注意：git clone　可能不好用，建议点击链接进行下载

cd ~/moveit_ws/
wget https://raw.githubusercontent.com/ros-gbp/ompl-release/debian/melodic/xenial/ompl/package.xml
sudo catkin make
source ~/ws_moveit/devel/setup.bash

2.2测试是否安装成功

roslaunch panda_moveit_config demo.launch

常见报错：

Could NOT find CUDA: Found unsuitable version "10.2", but required is exact
  version "10.0" (found /usr/local/cuda-10.0)

切换cuda版本即可：

见cmake - OpenCV claims to find "wrong" cuda version - Stack Overflow

CUDA多个版本的切换----亲测可用_OscarMind的博客-CSDN博客_cuda多个版本

sudo rm -rf cuda
sudo ln -s /usr/local/cuda-[需要的版本] /usr/local/cuda
cd ~/moveit_ws/
sudo rm -rf build
sudo catkin build

3.自定义规划算法

(1)在路径（～/moveit_ws/src/ompl/src/ompl/geometric/planners/rrt/src）复制TRRT.cpp,并重命名为TORRT.cpp。在路径（～/moveit_ws/src/ompl/src/ompl/geometric/planners/rrt）复制TRRT.h,并重命名为TORRT.h；

(2)使用ctrl+F将TORRT.cpp和TORRT.h文件中的TRRT替换为TORRT

(3)在路径（～moveit_ws/src/moveit/moveit_planners/ompl/ompl_interface/src）中找到planning_context_manager.cpp,添加头文件

#include <moveit/ompl_interface/planning_context_manager.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/profiler/profiler.h>
#include <utility>

#include <ompl/geometric/planners/rrt/TORRT.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/rrt/pRRT.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/rrt/TRRT.h>
#include <ompl/geometric/planners/rrt/LazyRRT.h>
#include <ompl/geometric/planners/est/EST.h>
#include <ompl/geometric/planners/sbl/SBL.h>
#include <ompl/geometric/planners/sbl/pSBL.h>
#include <ompl/geometric/planners/kpiece/KPIECE1.h>
#include <ompl/geometric/planners/kpiece/BKPIECE1.h>

添加接口

void ompl_interface::PlanningContextManager::registerDefaultPlanners()
{
  registerPlannerAllocator(  //
      "geometric::RRT",      //
      std::bind(&allocatePlanner<og::RRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
  registerPlannerAllocator(     //
      "geometric::RRTConnect",  //
      std::bind(&allocatePlanner<og::RRTConnect>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
  registerPlannerAllocator(  //
      "geometric::LazyRRT",  //
      std::bind(&allocatePlanner<og::LazyRRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
  registerPlannerAllocator(  //
      "geometric::TRRT",     //
      std::bind(&allocatePlanner<og::TRRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
  registerPlannerAllocator(  //
      "geometric::TORRT",     //
      std::bind(&allocatePlanner<og::TORRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

(4) 打开/home/arl/ws_moveit/src/panda_moveit_config/config/ompl_planning.yaml

改三处

第一处

planner_configs:
  SBL:
    type: geometric::SBL
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  EST:
    type: geometric::EST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
  LBKPIECE:
    type: geometric::LBKPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  BKPIECE:
    type: geometric::BKPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  KPIECE:
    type: geometric::KPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  RRT:
    type: geometric::RRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
  RRTConnect:
    type: geometric::RRTConnect
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  RRTstar:
    type: geometric::RRTstar
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
  TRRT:
    type: geometric::TRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    max_states_failed: 10  # when to start increasing temp. default: 10
    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
    init_temperature: 10e-6  # initial temperature. default: 10e-6
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
  TORRT:
    type: geometric::TORRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    max_states_failed: 10  # when to start increasing temp. default: 10
    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
    init_temperature: 10e-6  # initial temperature. default: 10e-6
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()

第二和三处

panda_arm:
  default_planner_config: RRT
  planner_configs:
    - SBL
    - EST
    - LBKPIECE
    - BKPIECE
    - KPIECE
    - RRT
    - RRTConnect
    - RRTstar
    - TRRT
    - TORRT
    - PRM
    - PRMstar
    - FMT
    - BFMT
    - PDST
    - STRIDE
    - BiTRRT
    - LBTRRT
    - BiEST
    - ProjEST
    - LazyPRM
    - LazyPRMstar
    - SPARS
    - SPARStwo
  projection_evaluator: joints(panda_joint1,panda_joint2)
  longest_valid_segment_fraction: 0.005
hand:
  default_planner_config: RRT
  planner_configs:
    - SBL
    - EST
    - LBKPIECE
    - BKPIECE
    - KPIECE
    - RRT
    - RRTConnect
    - RRTstar
    - TRRT
    - TORRT
    - PRM
    - PRMstar
    - FMT
    - BFMT
    - PDST
    - STRIDE
    - BiTRRT
    - LBTRRT
    - BiEST
    - ProjEST
    - LazyPRM
    - LazyPRMstar
    - SPARS
    - SPARStwo

(5)

cd ~/moveit_ws/
sudo catkin make

报错：Errors << moveit_planners_ompl:make /home/arl/ws_moveit/logs/moveit_planners_ompl/build.make.020.log

Errors     << moveit_planners_ompl:make /home/arl/ws_moveit/logs/moveit_planners_ompl/build.make.020.log                                                                                                   
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::~TORRT()’未定义的引用
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::TORRT(std::shared_ptr<ompl::base::SpaceInformation> const&)’未定义的引用
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/moveit_planners_ompl/moveit_ompl_planner] Error 1
make[1]: *** [ompl_interface/CMakeFiles/moveit_ompl_planner.dir/all] Error 2
make[1]: *** 正在等待未完成的任务....
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::~TORRT()’未定义的引用
/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/libmoveit_ompl_interface.so.1.0.8：对‘ompl::geometric::TORRT::TORRT(std::shared_ptr<ompl::base::SpaceInformation> const&)’未定义的引用
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/moveit_planners_ompl/generate_state_database] Error 1
make[1]: *** [ompl_interface/CMakeFiles/moveit_generate_state_database.dir/all] Error 2
make: *** [all] Error 2
cd /home/arl/ws_moveit/build/moveit_planners_ompl; catkin build --get-env moveit_planners_ompl | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

解决办法：

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

cd /home/arl/ws_moveit/devel/.private/moveit_planners_ompl/lib/
sudo rm -rf libmoveit_ompl_interface.so.1.0.8
cd ~/moveit_ws/
sudo catkin make

再运行demo就可以了

roslaunch panda_moveit_config demo.launch

后记：官方安装教程：

MoveIt 1 Source Build - Linux | MoveIt

Dependencies Source Install | MoveIt

_Never_stop_

关注

8
点赞
踩
81

收藏

觉得还不错? 一键收藏
13
评论
MoveIt自定义路径规划算法（melodic版本）

moveit自定义路径规划算法，终于实现了，花了五天时间，一直解决不了bug，下面讲讲如何在moveit中自定义路径规划算法1.源码下载moveit1.1.删除通过binary安装的moveitsudo apt-get remove ros-melodic-moveit-*1.2.更新软件包rosdep updatesudo apt-get updatesudo apt-get dist-upgrade常见bug：rosdep update会报错，更新不了，一定能解决的方法
复制链接

扫一扫