文档维护：Arvin

网页部署：Arvin

▶

写在前面：pluginlib是一个c++库， 用来从一个ROS功能包中加载和卸载插件(plugin)。插件是指从运行时库中动态加载的类。通过使用Pluginlib，不必将某个应用程序显式地链接到包含某个类的库，Pluginlib可以随时打开包含类的库，而不需要应用程序事先知道包含类定义的库或者头文件。

例如：目前我们想在ROS中实现自己的路径规划算法，就需要以plugin的方式。

下面本文将逐步介绍如何实现A*算法作为ROS中的全局路径规划。

0

编写the Path Planner Class

这里我们采用A*算法，其伪代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
* 初始化open_set和close_set；
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
    * 如果节点n为终点，则：
        * 从终点开始逐步追踪parent节点，一直达到起点；
        * 返回找到的结果路径，算法结束；
    * 如果节点n不是终点，则：
        * 将节点n从open_set中删除，并加入close_set中；
        * 遍历节点n所有的邻近节点：
            * 如果邻近节点m在close_set中，则：
                * 跳过，选取下一个邻近节点
            * 如果邻近节点m也不在open_set中，则：
                * 设置节点m的parent为节点n
                * 计算节点m的优先级
                * 将节点m加入open_set中

编写头文件和源文件

进入你的工作空间：

1
cd catkin_ws/src

创建一个功能包：

1
catkin_create_pkg astar_planner nav_core roscpp rospy std_msgs

然后在次功能包的src目录中创建两个文件：astar_planner.h和astar_planner.cpp

编写头文件astar_planner.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>

#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include <fstream>

#include <vector>
#include <queue>
#define infinity 1.0e10
using namespace std;

struct Node
{
    float cost;
    int index;
};

namespace astar_planner
{
    class AstarPlanner : public nav_core::BaseGlobalPlanner
    {
    public:
        AstarPlanner();
        AstarPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros);

        /**
         * @brief  Initialization function for the DijstraPlanner
         * @param  name The name of this planner
         * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
         */
        void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);

        /**
         * @brief Given a goal pose in the world, compute a plan
         * @param start The start pose
         * @param goal The goal pose
         * @param plan The plan... filled by the planner
         * @return True if a valid plan was found, false otherwise
         */
        bool makePlan(const geometry_msgs::PoseStamped &start,
                      const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan);

        int width;
        int height;
        int map_size;
        vector<bool> OGM;

        double getHeuristic(int cell_index, int goal_index);

        vector<int> get_neighbors(int current_cell);
        double getMoveCost(int firstIndex, int secondIndex);

        bool isInBounds(int x, int y);
        /**
         * @brief  Publish a path for visualization purposes
         */
        void publishPlan(const std::vector<geometry_msgs::PoseStamped> &path);

        ros::Publisher plan_pub_;
        std::string frame_id_;
        bool initialized_;
        costmap_2d::Costmap2DROS *costmap_ros_;
        costmap_2d::Costmap2D *costmap_;
    };
};

<costmap 2d/costmap 2d ross .h>和<costmap 2d/costmap 2d.h>来使用将被路径规划器用作输入映射的costmap 2d::Costmap2D类。当定义为插件时，路径规划器类将自动访问该地图。不需要订阅costmap2d来从ROS获取成本映射。<nav core/base global planner.h>用于导入接口nav core::BaseGlobalPlanner，插件必须遵守该接口。

为类定义名称空间是一个很好的实践，尽管不是必需的。这里，我们将命名空间定义为GlobalPlanner类的全局规划器。名称空间用于定义对类的完整引用，如global planner::GlobalPlanner。然后定义类GlobalPlanner并从接口nav core:: basegglobalplanner继承。在nav core:: basegglobalplanner中定义的所有方法必须被新类GlobalPlanner覆盖。

构造函数GlobalPlanner(std::string name, costmap 2d::Costmap2DROS* costmap ros)用于初始化成本映射，这是将用于规划的映射(costmap ros)，以及计划器的名称(name)。默认构造函数GlobalPlanner()也是如此，它用默认值初始化规划器属性。方法initialize(std::string name, costmap 2d::Costmap2DROS* costmap ros)是BaseGlobalPlanner的一个初始化函数，它初始化成本映射，即将用于规划的映射(costmap ros)，以及计划器的名称(name . string)。

编写源文件astar_planner.cpp

参考链接：https://github.com/hubery05/Astar_Path_Planner

编译

在Cmakelist中添加:

1
add_library(astar_planner_lib src/astar_planner.cpp)

然后在工作空间catkin_make，这将会在目录~/catkin_ws/devel/lib中创建.so文件。

2

编写Plugin

Plugin Registration

在astar_planner.cpp中添加以下代码，为了允许动态加载类，必须将其标记为导出类。

1
PLUGINLIB_EXPORT_CLASS(astar_planner::AstarPlanner, nav_core::BaseGlobalPlanner)

Plugin Description File

在功能包目录中添加描述文件

1
2
3
4
5
6
<library path="lib/libastar_planner_lib">
    <class name="astar_planner/AstarPlanner" type="astar_planner::AstarPlanner"
        base_class_type="nav_core::BaseGlobalPlanner">
        <description>This is a astar global planner plugin.</description>
    </class>
</library>

<library path="lib/libastar_planner_lib">–指定生成插件的库目录，默认在~/catkin_ws/devel/lib
指定name="astar_planner/AstarPlanner"用于nav_core调用，一般格式为namespace + class_name

Registering Plugin with ROS Package System

在功能包的package.xml中添加：

1
2
3
4
5
<export>
    <!-- Other tools can request additional information be placed here -->
    <nav_core plugin="${prefix}/relaxed_astar_planner_plugin.xml" />

</export>

查看ROS路径规划可用插件

1
rospack plugins --attrib=plugin nav_core

1

调用插件

修改move_base_params.yaml中base_global_planner的name。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
base_global_planner: "astar_planner/AstarPlanner"
base_local_planner: "base_local_planner/TrajectoryPlannerROS"
base_local_planner: "dwa_local_planner/DWAPlannerROS"
clearing_rotation_allowed: true
conservative_reset_dist: 3.0
controller_frequency: 20.0
controller_patience: 5.0

make_plan_add_unreachable_goal: true
make_plan_clear_costmap: true
max_planning_retries: -1
oscillation_distance: 0.5
oscillation_timeout: 0.0
planner_frequency: 0.0
planner_patience: 5.0
recovery_behavior_enabled: true
restore_defaults: false
shutdown_costmaps: false
 
# 
# base_global_planner: "global_planner/GlobalPlanner"

然后启动导航包即可。

3

DWA_local_planner

1
catkin_create_pkg dwa_local_planner angles base_local_planner cmake_modules costmap_2d dynamic_reconfigure nav_core nav_msgs pluginlib sensor_msgs roscpp tf2 tf2_geometry_msgs tf2_ros

参考链接：

ROS官方插件教程

在ROS中实现A*路径规划

A*算法