ROS kinetic自定义路径规划算法

---

前言

本文章使用古月居中的ROS移动机器人仿真环境，通过对Navigation源码的编译，实现利用自己的路径规划算法进行路径规划的目的。在该篇文章中以移植的为RRT*算法为例，该算法的特点读者可以自行百度学习了解，

---

一、Navigation源码的编译

首先根据的自己的ROS版本下载相应版本的Navigation源码（如果已安装二进制版的导航功能包需将其卸载），作者使用的是kinetic版本的Navigation，源码链接，也可以自行去github搜索下载。

1.Navigation二进制版的卸载

在终端中执行如下指令：
首先安装依赖项

sudo apt-get install ros-kinetic-navigation*

移除原有的navigation功能包

sudo apt-get remove ros-kinetic-navigation 
sudo apt-get remove ros-kinetic-navigation-experimental

然后再到工作空间中下载navigation，或者直接将其拷贝在（工作空间/src）文件夹下,执行

catkin_make

2.Navigation源码的安装步骤

具体的安装步骤可参考这篇文章，或许有可能报错可以根据提示安装对应的依赖即可。

二、RRT*路径规划算法的移植

1.RRT*路径规划插件

该部分的源码见链接。在下载的功能包中内容如下：
其中src和include文件夹中分别存放源文件和头文件。需要注意的有如下几点：

(1)在CMakeLists.txt文件中：

add_library(${PROJECT_NAME}
   src/RRTstar_ros.cpp
)

执行这条语句将会在工作空间/devel/lib中编译生成相应的动态链接库——lib(工程名字).so，里面PROJECT_NAME为工程名，当然也可以自己定义，但是为了方便和区分一般会使用工程名。
(2)编辑插件描述文件
在RRT*功能包中有一个名为：rrt_star_planner_plugin.xml的文件。内容如下：

<library path="lib/librrt_star_global_planner">
  <class name="RRTstar_planner/RRTstarPlannerROS" type="RRTstar_planner::RRTstarPlannerROS" base_class_type="nav_core::BaseGlobalPlanner">
    <description>This is RRT Star Global Planner Plugin by Rafael Barreto.</description>
  </class>
</library>

其中library path描述该功能包到library的相对路径；class name为插件名字，插件名字通常使用命名空间/类名称的形式进行命名，如（RRTstar_planner/RRTstarPlannerROS）；type表示功能类的类型全名；base_class_type表示实现插件的基类名称；description描述功能类的功能。有关该文件的详细信息可以参考该文档。
（3）在ROS包系统中注册路径规划插件
在package.xml文件中的末尾有如下语句：

<export>
    <!-- Other tools can request additional information be placed here -->
   <nav_core plugin="${prefix}/rrt_star_planner_plugin.xml"/>

  </export>

其中${prefix}/将会自动确定rrt_star_planner_plugin.xml文件的完整路径。该文件的详细描述可参考这篇文档。
在对功能包进行编译完成后可通过如下指令进行查看路径规划算法是否成功注册到ROS系统中。

rospack plugins --attrib=plugin nav_core

成功则可以从列表中看到自己定义的路径规划插件，如下图：

三、在机器人仿真环境下的调用

本文以古月居论坛中提供的ROS仿真机器人为例，对其用到的路径规划方法进行修改，以到达到在ROS中使用自定义路径规划方法的目的。该部分主要分为以下几步：
1、在工作空间目录下的src/mrobot_navigation/mrobot目录下分别创建名为base_global_planner_params.yaml和move_base_params.yaml的两个文件。
（1）base_global_planner_params.yaml内容为：

GlobalPlanner:
  allow_unknown: false
  default_tolerance: 0.2
  visualize_potential: false
  use_dijkstra: false
  use_quadratic: true
  use_grid_path: false
  old_navfn_behavior: false
  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0
  publish_potential: true
  orientation_mode: 0
  orientation_window_size: 1

（2）move_base_params.yaml的内容为：

#base_global_planner: global_planner/GlobalPlanner
base_global_planner: RRTstar_planner/RRTstarPlannerROS
#base_global_planner: astar_planner/AstarPlanner
base_local_planner: dwa_local_planner/DWAPlannerROS

shutdown_costmaps: false

controller_frequency: 5.0 #before 5.0
controller_patience: 3.0

planner_frequency: 0.5
planner_patience: 5.0

oscillation_timeout: 10.0
oscillation_distance: 0.2

conservative_reset_dist: 0.1 #distance from an obstacle at which it will unstuck itself

cost_factor: 1.0
neutral_cost: 55
lethal_cost: 253

这里面一些具体的参数设置读者可以去自行百度搜索查阅，在这里就不做赘述了。
2、对工作空间目录下的src/mrobot_navigation/launch目录下的move_base.launch文件进行修改，该步主要是将第一步中创建的文件目录添加的move_base.launch文件中。修改完成后内容如下：

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find mrobot_navigation)/config/mrobot/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mrobot_navigation)/config/mrobot/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mrobot_navigation)/config/mrobot/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mrobot_navigation)/config/mrobot/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find mrobot_navigation)/config/mrobot/base_local_planner_params.yaml" command="load" />
    <rosparam file="$(find mrobot_navigation)/config/mrobot/base_global_planner_params.yaml" command="load" />
    <rosparam file="$(find mrobot_navigation)/config/mrobot/move_base_params.yaml" command="load" />
  </node>
  
</launch>

3、对工作空间进行编译，在编译完成后，分别在终端中输入以下指令：

roslaunch mrobot_gazebo mrobot_laser_nav_gazebo.launch
roslaunch mrobot_navigation fake_nav_cloister_demo.launch

有可能会出现如下图所示错误：
可以使用如下命令添加路径：

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:[path of your ros package]

例如：

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/zgd/ROS_Works/shenlan_ws/src

这时可能出现如下错误：
原因是因为在使用路径规划器进行路径规划时机器人长时间困在一小片区域，这时可以通过启动move_base的自恢复机制重新进行路径规划。主要对通过对move_base_params.yaml文件中的这两个参数进行调整：

oscillation_timeout: 0.0 #10.0
oscillation_distance: 0.5 #0.2

四、实验现象

Gazbo中的仿真环境：
rviz的仿真：
实验结果并不是很好，可能是路径规划器的一些参数还需修改，RRT*算法相比于astar算法花费的时间确实较长。

总结

该篇文章的主要目的主要是对自定义路径规划算法的移植进行了介绍，这其中所涉及路径规划算法的详细原理读者可以自行搜索查阅，由于作者学识水平有限，在文章中难免会出现欠妥的地方，希望大家指出一起学习。