​

         今天偶然发现了一个优质的运动规划库：ai-winter/ros_motion_planning，比较适合从事ROS移动机器人运动规划研究领域的小伙伴学习和使用，相比于莱斯大学Kavraki实验室提供的开源的著名运动规划库OMPL、或着我之前介绍过的zhm-real开源的zhm-real/MotionPlanning和zhm-real/PathPlanning运动规划库，今天介绍的ROS Motion Planning运动规划库与ROS机器人中常用的Navigation导航框架的兼容性更好，库中的运动规划算法采用了全局和局部规划器插件的形式，使用起来更加的简便快捷。本文主要包含ROS Motion Planning运动规划库简介、安装方法、进阶使用方法介绍等主要内容

一、ROS Motion Planning运动规划库简介

        ROS Motion Planning运动规划库包含了插件形式的 A*, JPS, D*, LPA*, D* Lite, Theta*, RRT, RRT*, RRT-Connect, Informed RRT*, ACO, Voronoi, PID, DWA, APF 等运动规划算法，ROS Motion Planning运动规划库的作者是优质技术博主Mr.Winter`，其博客及Github主页链接如下：

https://mr-winter.blog.csdn.net/

https://github.com/ai-winter       

     

二、ROS Motion Planning运动规划库安装方法

 1、创建新的ROS工作空间

        由于ROS Motion Planning运动规划库与Navigation导航包存在一些同名但源码和功能不同的功能包，比如global_planner，因此，个人推荐创建一个新的工作空间来存储ROS Motion Planning运动规划库，不要与Navigation导航包放在同一工作空间下，在本文的例子中，我将创建一个名为motionplanning_ws的工作空间，步骤如下

        注：工作空间名字可任取，将下面语句中的motionplanning_ws改为任取的名字即可

（1）、创建工作空间

        在终端依次执行以下三条语句创建名为motionplanning_ws的ROS工作空间

mkdir -p ~/motionplanning_ws/src

cd ~/motionplanning_ws/src

catkin_init_workspace

（2）、编译工作空间
         

        依次执行以下两条语句对该工作空间进行编译

cd ~/motionplanning_ws/

catkin_make

（3）、设置环境变量

source devel/setup.bash

（4）、检查环境变量

 echo $ROS_PACKAGE_PATH

（5）、添加环境变量至.bashrc文件

        在主目录（即home/用户名 文件夹）下，双击打开.bashrc文件，看不见就是被隐藏了，按Ctrl+h显示隐藏文件，就可以看到了，在.bashrc文件中添加该工作空间的环境变量，形式如下，其中source后面的路径就是工作空间motionplanning_ws的devel文件夹下的setup.bash文件所在的路径，大家需要根据自己实际情况修改，添加后保存关掉.bashrc文件，可以按Ctrl+h来隐藏此类文件，使得主目录看起来更简洁

source /home/gly/motionplanning_ws/devel/setup.bash

   至此，用于存储ROS Motion Planning运动规划库的ROS工作空间就准备好了

 2、下载/克隆ROS Motion Planning运动规划库

        可以执行以下两条指令来从Github上克隆ROS Motion Planning运动规划库至我们上一步创建的工作空间

cd /motionplanning_ws/src

git clone https://github.com/ai-winter/ros_motion_planning.git

        因为网络原因使用上述语句克隆失败的，可以手动去Github下载压缩包形式的ROS Motion Planning运动规划库，然后放到motionplanning_ws工作空间的src文件夹下，并解压
 

 3、安装相关依赖
   

（1）、关于Python  

        关于Python推荐使用Python3，对于Ubuntu20.04对应的ROS Noetic一般默认使用的都是Python3，可以在终端执行python指令来查看Python版本

        如果遇到Python command not found，可尝试运行以下指令

sudo apt install python-is-python3

（2）、相关依赖功能包

        如果之前使用过Navigation导航包，大概率以下依赖功能包已经安装过了，没有使用过的依次执行以下语句进行安装，非Noetic版本ROS的注意自行更改语句中的ROS版本

sudo apt install ros-noetic-amcl

sudo apt-get install ros-noetic-map-server

sudo apt install ros-noetic-move-base

sudo apt install ros-noetic-base-local-planner

sudo apt install ros-noetic-navfn

4、编译ROS Motion Planning运动规划库

        依次执行以下两条语句进行编译

cd /motionplanning_ws

catkin_make

5、简单验证ROS Motion Planning运动规划库是否可以正常工作

        在终端执行以下语句来运行作者提供的demo，配合rviz界面的2D New Goal 指定目标点来初步验证ROS Motion Planning运动规划库是否安装成功，并可正常工作。

roslaunch sim_env main.launch 

        动态效果演示如下：（↓↓↓↓↓点击以下图片查看动态效果演示↓↓↓↓↓）

三、ROS Motion Planning运动规划库进阶使用方法

        ROS Motion Planning运动规划库采用了非常灵活和简易的方式来按照我们的需求去进行配置和使用，在我们对一些必要参数进行设定后，可使用库中的main_generate.py文件自动生成我们设定的参数对应的执行文件main.launch。

        这些需要设定的参数，存放在user_config.yaml、pedestrians_config.yaml、obstacles _config.yaml这三个文件中，这三个文件均位于下图所示的目录下，下面依次进行详细的介绍

1、编写user_config.yaml参数配置文件

        首先，我们来看作者提供的示例程序，如下所示：

map: "warehouse"
world: "warehouse"
rviz_file: "sim_env.rviz"

robots_config:
  - robot1_type: "turtlebot3_waffle"
    robot1_global_planner: "theta_star"
    robot1_local_planner: "apf"
    robot1_x_pos: "0.0"
    robot1_y_pos: "0.0"
    robot1_z_pos: "0.0"
    robot1_yaw: "0.0"

# plugins:
  # pedestrians: "pedestrian_config.yaml"
  # obstacles: "obstacles_config.yaml"

        接下来，我们对每个参数的作用以及可选的选项有那些进行逐个介绍

（1）、map

        map参数用于设定运动规划所使用的静态地图的名字，也就是rviz界面看到的静态全局地图，这些可用的静态地图文件存放在该运动规划库的src/sim_env/map/文件夹下，目前该运动规划库中仅提供了名为warehouse的静态地图文件，如下图所示，当然，我们可以利用SLAM建图算法自行生成其他的静态地图文件，然后同样存放在src/sim_env/map/文件夹下，并将参数map设定为该静态地图的名字，即可进行调用。

         注：该参数设为“ ”，则表示不使用静态地图

（2）、world

        world参数用于设定要调用的Gazebo仿真环境的文件名，该文件存放在该运动规划库的src/sim_env/worlds/文件夹下，示例中调用的是warehouse.world。当然，我们也可以在Gazebo中搭建自己的仿真环境，并导出对应的world文件，然后存放到该目录下，通过修改参数world来进行调用
         注：需要注意的是参数map与world要对应起来，这里的对应是指环境内容的对应，一般是先在Gazebo中搭建仿真环境，保存为xxx.world文件，然后使用SLAM建图算法，比如最常用的gampping，对该仿真环境进行建图，将建好的图作为静态地图保存，然后使用。

（3）、rviz_file

        rviz_file参数用于设定打开rviz时的配置文件，这些配置文件存放在src/sim_env/rviz/目录下，目前库中提供了cache.rviz、sim_env.rviz、view_multi.rviz这三个可选内容，同样我们可以在rviz中自行调整，并导出对应.rviz文件，存储在该目录下，然后通过对rviz_file参数进行修改来调用

        比如，我修改了全局路径的话题及线型等，可以直接save config覆盖原有设置，也可以导出新的.rviz配置文件，从而保留原有设置，如下图所示：

（4）、robot1_type

        robot1_type用于设定编号为robot1的机器人所使用的机器人的模型，模型文件在库的src/sim_env/urdf/目录下库中提供了nanocar、turtlebot3_burger、turtlebot3_waffle 、turtlebot3_waffle_pi四种可选的机器人模型，当然，我们也可以自行创建或者从网上找其他的机器人模型的urdf或xacro文件，放到该目录下，通过修改参数robot1_type的值来进行调用。

        库中提供的这四种机器人模型如下图所示：

（5）、robot1_global_planner

        robot1_global_planner用于设定编号为robot1的机器人所使用的全局路径算法，全局路径算法的实现文件在库的src/planner/global_planner/目录下，库中提供了三大类共同16个全局路径规划算法，对应三个全局路径规划器插件，详细如下：

        ① 基于图搜索的全局路径规划器插件graph_planner/GraphPlanner包含的算法有：

          a_star、jps、gbfs、dijkstra、d_star、lpa_star、voronoi、d_star_lite、theta_star、lazy_theta_star

        ② 基于采样的全局路径规划器插件sample_planner/SamplePlanner包含的算法有：

        rrt、rrt_star、informed_rrt、rrt_connect

        ③、基于智能的路径规划器插件evolutionary_planner/EvolutionaryPlanner包含的算法有：

        aco、ga

        三大类共16种全局路径规划算法可供我们选择，上面的示例文件中使用的是theta_star算法，我们可以尝试修改为以上其他16种算法中的任意一个来测试其效果，当然，我们也可以编写自己的全局路径规划器算法，较容易的方式，是直接在以上16种算法中最接近的1种的源文件上进行修改，若创建自己的算法名和源文件，则还需要修改库的src/sim env/ launch /include /navigation/目录下的move base.launch.xml等文件

（6）、robot1_local_planner

         robot1_local_planner用于设定编号为robot1的机器人所使用的局部路径算法，局部路径算法的实现文件在库的src/planner/local_planner/目录下，库中提供了四个局部路径规划算法，对应四个局部路径规划器插件，详细如下：

        ①、插件dwa_planner/DWAPlanner对应的算法是：dwa

        ②、插件pid_planner/PIDPlanner对应的算法是：pid

        ③、插件apf_planner/APFPlanner对应的算法是：apf

        ④、插件static_planner/StaticPlanner对应的算法是：static

        四个局部路径规划器插件对应4种局部路径规划算法可供我们选择，上面的示例文件中使用的是apf算法，我们可以尝试修改为以上其他4种算法中的任意一个来测试其效果，当然，我们也可以编写自己的全局路径规划器算法，较容易的方式，是直接在以上4种算法中最接近的1种的源文件上进行修改，若创建自己的算法名和源文件，则还需要修改库的src/sim env/ launch /include /navigation/目录下的move base.launch.xml等文件

（7）、robot1_x_pos、robot1_y_pos、robot1_z_pos、robot1_yaw

        robot1_x_pos、robot1_y_pos、robot1_z_pos、robot1_yaw分别用于设定编号为robot1的机器人的初始位置和姿态角，对于位置设定为地图中任意一个非障碍物点都行，经过我的测试，目前不支持任意修改初始姿态角robot1_yaw，其值保持为0即可，若修改为其他值rviz中显示的雷达坐标系将出现问题。

（8）、行人插件 pedestrians

        除了上述必备的配置参数外，库中还提供了行人插件pedestrians，若想启用该功能，则需要取消对该插件的注释，如下所示，该插件可以在仿真环境中添加动态的行人来充当动态障碍物，对规划算法的性能进行测试，使用该插件同样需要提供一个名为pedestrian_config.yaml参数配置文件，将在本文第三部分的第2小节，也就是下一小节中对该参数配置文件进行详细的介绍。

plugins:
  pedestrians: "pedestrian_config.yaml"

（9）、新增障碍物插件 obstacles_config

         除了行人插件，库中还提供了新增障碍物插件 obstacles_config，若想启用该功能，则需要取消对该插件的注释，如下所示，该插件可以在仿真环境中添加新的静态障碍物，新增的障碍物在SLAM建图产生的静态地图map中并不存在，因此，可以用来测试规划算法对新感知到的障碍物的避障能力，使用该插件同样需要提供一个名为obstacles_config.yaml参数配置文件，将在本文第三部分的第3小节中对该参数配置文件进行详细的介绍。

plugins:
  obstacles: "obstacles_config.yaml"

        如下图所示的Gazebo环境中新增了蓝框内静态障碍物，对应的rviz地图中的位置如红框所示，在机器人搭载的激光雷达等传感器感知到该障碍物之间，并不会添加至代价地图中

2、编写pedestrian_config.yaml参数配置文件

        按照自己的需求编写或修改完user_config.yaml参数配置文件后，若在该文件中启用了行人插件 pedestrians，则需要进一步根据我们的需求编写pedestrian_config.yaml参数配置文件，我们先来看一下，作者提供的参考示例，如下所示：

# sfm algorithm configure
social_force:
  animation_factor: 5.1
  # only handle pedestrians within `people_distance`
  people_distance: 6.0
  # weights of social force model
  goal_weight: 2.0
  obstacle_weight: 20.0
  social_weight: 15
  group_gaze_weight: 3.0
  group_coh_weight: 2.0
  group_rep_weight: 1.0

# pedestrians setting
pedestrians:
  update_rate: 5
  ped_property:
    - name: human_1
      pose: 3 2 1 0 0 0
      velocity: 0.9
      radius: 0.4
      cycle: true
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: 3 2 1 0 0 0
        goal_point_2: 3 -8 1 0 0 0
    - name: human_2
      pose: -0.5 -8.5 1 0 0 0
      velocity: 1.2
      radius: 0.4
      cycle: true
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: -0.5 -8.5 1 0 0 0
        goal_point_2: -9 2.5 1 0 0 0
    - name: human_3
      pose: -3.5 -8.5 1 0 0 0
      velocity: 1.2
      radius: 0.4
      cycle: true
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: -3.5 -8.5 1 0 0 0
        goal_point_2: -3.5 8.5 1 0 0 0

        接下来，我们对其进行详细的介绍，我们先来看群体参数social_force部分

（1）、people_distance

        参数people_distance用于设定在社会行为决策时考虑多大范围内的其他行人，上述配置文件中设定值为6，即每个行人对其行为进行决策时会考虑距离其6m范围内的其他行人

（2）、goal_weight、obstacle_weight、social_weight

         参数goal_weight、obstacle_weight、social_weight分别用于设定行人在进行行为决策时前往设定目标点的引力权重、避障障碍物权重、远离其他行人的斥力权重。

（3）、group_gaze_weight、group_coh_weight、group_rep_weight

        参数group_gaze_weight、group_coh_weight、group_rep_weight分别用于设定行人通过调整位姿减少头部旋转角度的权重、与群体中其他行人一同行走的权重（引力）、与群体中其他行人不相互重叠的斥力权重        

  注：social_force部分参数的详细介绍可见插件的官方介绍，链接如下：

     https://github.com/robotics-upo/lightsfm

 接下来，我们再来看设定每个行人的参数，即pedestrians部分

（4）、update_rate

        参数update_rate用于设定最新数据的更新率

（5）、- name

        参数- name是每个行人的第一个具体参数，用于设定该行人的名字，如human_1、human_2、human_3等来区分每个行人

（6）、pose

        参数pose用来设定该行人的起始位姿，前三个元素是起始位置坐标，后三个元素是起始姿态

（7）、velocity

        参数velocity用来设定该行人的最大速度

（8）、radius

        参数radius用于设定人体的大约半径

（9）、cycle

        参数cycle用于设定该行人是否启用循环行走，即当机器人达到目标点队列中最后一个目标点后，是否继续从第一个目标点开始循环执行下去。

（10）、time_delay

        参数time_delay用于设定该行人开始运动的延时时间，即仿真环境运行多少秒后，机器人才开始运动，若不对该参数进行设定，默认不延时，即为0秒，仿真环境运行后该行人就开始前往目标点队列中的第一个点

（11）、ignore

        参数ignore用于设定该行人在进行行为决策时忽略的障碍物，上述的例子中行人1~3，都忽略了地板ground_plane和名为turtlebot3_waffle的机器人，地板当然不能视为障碍物，这个很好理解，不将机器人视为障碍物是为了检验规划算法的与人友好性，即当与行人相遇时，我们期望机器人主动避让行人，所以当将机器人的名字放在某个行人的ignore参数下时，该行人遇到机器人时，即使发生碰撞，不会进行避让。

（12）、trajectory

        参数trajectory用于设定该行人的目标点队列，比如若期望该行人先前往目标点1，再前往目标点2，这两个目标点的位姿分别为3 2 1 0 0 0和3 -8 1 0 0 0，则trajectory可设置为

      trajectory:
        goal_point_1: 3 2 1 0 0 0
        goal_point_2: 3 -8 1 0 0 0

        接下来，我们再来看一个实例来加深对以上参数的理解，在下面的例子中，我在上述示例的基础上添加了行人4，其初始位姿为0 0 1 0 0 0，最大行走速度为2.5m/s，人体半径大约为0.4，启用循环行走，延时5秒后，开始行走，并在避障时忽略地板，但不忽略机器人，即会主动避让机器人，其目标点队列含有-3.4 -5.7 1 0 0 0、-3.4 -8.5 1 0 0 0、0 0 1 0 0 0这三个目标点，由于启用了循环行走，即该行人会不断以此前往这三个点，循环执行下去。

# sfm algorithm configure
social_force:
  animation_factor: 5.1
  # only handle pedestrians within `people_distance`
  people_distance: 6.0
  # weights of social force model
  goal_weight: 2.0
  obstacle_weight: 20.0
  social_weight: 15
  group_gaze_weight: 3.0
  group_coh_weight: 2.0
  group_rep_weight: 1.0

# pedestrians setting
pedestrians:
  update_rate: 5
  ped_property:
    - name: human_1
      pose: 3 2 1 0 0 0
      velocity: 0.9
      radius: 0.4
      cycle: true
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: 3 2 1 0 0 0
        goal_point_2: 3 -8 1 0 0 0
    - name: human_2
      pose: -0.5 -8.5 1 0 0 0
      velocity: 1.2
      radius: 0.4
      cycle: true
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: -0.5 -8.5 1 0 0 0
        goal_point_2: -9 2.5 1 0 0 0
    - name: human_3
      pose: -3.5 -8.5 1 0 0 0
      velocity: 1.2
      radius: 0.4
      cycle: true
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: -3.5 -8.5 1 0 0 0
        goal_point_2: -3.5 8.5 1 0 0 0
    - name: human_4
      pose: 0 0 1 0 0 0
      velocity: 2.5
      radius: 0.4
      cycle: true
      time_delay: 5
      ignore:
        model_1: ground_plane
      trajectory:
        goal_point_1: -3.4 -5.7 1 0 0 0
        goal_point_2: -3.4 -8.5 1 0 0 0
        goal_point_3:  0 0 1 0 0 0

         运行效果如下所示，从下面的动图可以看出，避障时行人3由于设定为忽略机器人，因此遇到机器人时即使会碰撞也不会避让，所以直接踩过去了，行人4由于设定了避障时考虑机器人，所以胡主动避让机器人，然而机器人位于可行区域中心，行人半径设为0.4，其想避让但过不去，会在原地徘徊，直到机器人向左侧移动后，行人4才从其右侧通行

                                                                                     注：↓↓↓↓↓↓点击以下图片查看动态演示图↓↓↓↓↓↓

3、编写obstacles_config.yaml参数配置文件

        按照自己的需求编写或修改完user_config.yaml参数配置文件后，若在该文件中启用了新增障碍物插件 obstacles，则需要进一步根据我们的需求编写obstacles_config.yaml参数配置文件，我们先来看一下，作者提供的参考示例，如下所示：

obstacles:
  - type: BOX
    pose: 4 2 0 0 0 0
    color: Grey
    props:
      m: 1.00
      w: 0.50
      d: 1.00
      h: 0.80
  - type: BOX
    pose: 3 3 0 0 0 0
    color: Grey
    props:
      m: 1.00
      w: 1.00
      d: 0.50
      h: 1.00

（1）、type

        参数type用于设定障碍物的形状类型，其值可设为BOX、CYLINDER 或SPHERE，分别对应长方形、圆柱形和球形。

（2）、color

        参数color用于设定该障碍物的颜色，可选颜色如下图所示

        详情可见如下链接：http://wiki.ros.org/simulator_gazebo/Tutorials/ListOfMaterials

（3）、props

        参数props用于设定该障碍物尺寸和质量，其中m设定质量、w设定宽度、d设定深度/厚度、h设定高度、r设定半径

        上面的示例程序设定了两个长方体新增障碍物，我对其进行了修改，新增了一个圆柱体障碍物和一个球体障碍物，新的示例程序如下：

# static obstacles
obstacles:
  - type: BOX
    pose: 4 2 0 0 0 0
    color: Grey
    props:
      m: 1.00
      w: 0.50
      d: 1.00
      h: 0.80
  - type: BOX
    pose: 3 3 0 0 0 0
    color: Grey
    props:
      m: 1.00
      w: 1.00
      d: 0.50
      h: 1.00
  - type: CYLINDER
    pose: 4 6 0 0 0 0
    color: Red
    props:
      m: 1.00
      r: 0.50
      h: 3.00
  - type: SPHERE
    pose: 4 6 4 0 0 0
    color: XYZPoints
    props:
      m: 1.00
      r: 1.0

        这四个新增的障碍物分别如下图中编号为①~④的物体所示所示：

4、运行main_generate.py文件根据上述设定的配置参数生成执行文件main.launch。

        根据我们自己的需求设定完上述参数配置文件后，就可以借助库中提供的main_generate.py文件，使用我们提供的参数配置文件，自动生成执行文件main.launch了，具体操作步骤如下：

        在该库的src/third_party/dynamic_xml_config/路径下，也就是main_generate.py文件所在的位置，空白处点击鼠标右键，选择打开终端，然后在打开的终端中执行以下语句

python main_generate.py user_config.yaml

          执行成功后，不会打印任何信息，如下图所示：

5、运行main.launch文件，进行测试

        成功生成main.launch文件后，就可以运行以下语句来对其进行测试了

roslaunch sim_env main.launch 

         按照上述介绍的新增四个静态障碍物和四个动态行人的环境下，效果演示如下，其中全局和局部规划算法分别设定的theta_star和apf

         演示视频链接:https://www.bilibili.com/video/BV1hN411G7mh/

​