ROS中编写自己全局路径规划插件实现固定路线规划（1）

---

前言

之前文章介绍了怎样读取机器人移动轨迹并在rviz中显示读取机器人移动轨迹，本文尝试将读取的移动轨迹作为机器人的全局路径，以便接下来使用Navigation库进行导航任务。

---

一、注册全局路径插件

在ROS运动规划学习五—global_planner中介绍了全局路径规划的实现，需要注册全局路径插件，使其成为ros插件，在ros中使用；具体过程也可以参考创客智造的文章：ROS与navigation教程-编写自定义全局路径规划

二、实现过程

1.global_path_planner.h实现

#ifndef PROJECT_GLOBAL_PATH_PLANNER_H
#define PROJECT_RC_PATH_PLANNER_H

/*
 * 需要包含路径规划器所需的核心ROS库。
 * 路径规划器需要使用<costmap_2d/costmap_2d_ros.h>和<costmap_2d/costmap_2d.h>, 而costmap_2d::Costmap2D类作为输入的地图类。
 * 当定义为插件时，路径规划器类将自动访问此地图。没有必要订阅costmap2d来获取ROS的代价地图。
 * <nav_core/base_global_planner.h> 用于导入接口nav_core::BaseGlobalPlanner，这是插件必须要继承的父类。
*/
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.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 <move_base_msgs/MoveBaseActionResult.h>
#include <std_msgs/String.h>
#include <string.h>
/*
* 类定义命名空间,这里用global_path_planner表示，定义类GlobalPathPlanner并继承接口nav_core::BaseGlobalPlanner
*/
namespace global_path_planner{
    class GlobalPathPlanner : public nav_core::BaseGlobalPlanner
    {
    public:
    	//默认构造函数
    	GlobalPathPlanner();
    	//方法initialize是BaseGlobalPlanner的初始化函数，用于初始化costmap，参数为用于规划的代价地图（costmap_ros）和规划器的名称(name)
        void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
        //makePlan方法必须要重写。最终规划的路径将存储在方法的参数std::vector<geometry_msgs::PoseStamped>& plan 中。该规划将通过插件自动发布为话题。
        bool makePlan(const geometry_msgs::PoseStamped &start,const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan);
  
    private:
    	//以下写功能函数以及变量声明
    };
}

#endif //PROJECT_GLOBAL_PATH_PLANNER_H

2.global_path_planner.cpp实现

#include <pluginlib/class_list_macros.hpp>
#include <actionlib_msgs/GoalStatus.h>
#include <global_path_planner/global_path_planner.h>

PLUGINLIB_EXPORT_CLASS(global_path_planner::GlobalPathPlanner, nav_core::BaseGlobalPlanner)

namespace global_path_planner
{
    GlobalPathPlanner::GlobalPathPlanner()
    {
		...
    }
    void GlobalPathPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
    {
		...
    }

    bool GlobalPathPlanner::makePlan(const geometry_msgs::PoseStamped &start,
                      const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan)
    {
		...
		return true;
    }
}

3.添加相关配置

CMakeLists.txt文件

add_library(global_path_planner src/global_path_planner .cpp)
target_link_libraries(global_path_planner 
    ${catkin_LIBRARIES}
    )

package.xml文件

	...
  <build_depend>costmap_2d</build_depend>
  <build_depend>nav_core</build_depend>
  <build_depend>pluginlib</build_depend>
  <build_depend>roscpp</build_depend>
  <build_export_depend>costmap_2d</build_export_depend>
  <build_export_depend>nav_core</build_export_depend>
  <build_export_depend>pluginlib</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <exec_depend>costmap_2d</exec_depend>
  <exec_depend>nav_core</exec_depend>
  <exec_depend>pluginlib</exec_depend>
  <exec_depend>roscpp</exec_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->
    <nav_core plugin="${prefix}/global_path_planner_plugin.xml" />
  </export>

在功能包中，新建.xml文件，作为插件描述文件,用于以机器可读格式存储有关插件的所有重要信息。它包含有关插件库的信息，插件的名称，插件的类型等。以global_path_planner_plugin.xml为例。属性 path为插件库的路径，值为lib/libxxx,其中xxx是add_library下节点名字

 <library path="lib/libglobal_path_planner">
  <class name="global_path_planner/GlobalPathPlanner" type="global_path_planner::GlobalPathPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
         This is a global path planner plugin.
    </description>
  </class>
 </library>

4.编译查看插件注册

rospack plugins --attrib=plugin nav_core

会在终端打印我们自己的全局规划插件

---

总结

本文介绍了全局路径规划插件的注册过程，之后将实现固定路径作为全局路径发布，放一个效果图吧。