yabhoom智能机器人学习记录——（七）编写自定义的全局规划路径器-CSDN博客

本文链接：https://blog.csdn.net/OldUncleYan1983/article/details/136052748

老规矩，先来致谢，感谢B站视频上的雷达大叔，最近反反复复的看他的视频，受益匪浅，可能技术人员开发到一定程度，都是这样的心态，学习知识重要，但是学习如何有运用已有的知识解决问题更加重要。

另外，本文是参考wiki，Writing A Global Path Planner As Plugin in ROS这个文章，英语好的看原文，英语不好的看这个链接，感谢翻译君，我是对比的看的。

ROS与navigation教程-编写自定义全局路径规划 - 创客智造/爱折腾智能机器人

综述

首先要明确，我们的开发是基于ROS的开发，那么就要遵守ROS的规矩，正所谓我的规矩就是规矩。那么好，规矩是什么？对的就是引用ROS打好的框架。框架又是什么？是的MoveBase，也是很多书里面根本不会深入涉及的东西。相信ROS学到这里的人，应该都有一个感触，ROS好像没什么概念特别反人类，跟着一个视频学几天，好像通信建立就有了，也可以随便弄几个TF虎人，好像也可以弄几个launch文件，把自己几段丑陋的代码排列组合一下了。但是如何自己弄一个规划呢？看着AMCL加ROS自己的路径规划器，让自己花了钱买的车走成那个样子，打击的我半夜都做噩梦。

老人真是话多。。。。。。开始总结文章吧。

开始编写

第一步：引用类。

C++语言的三大特点，相信参加过面试的人或多或少的都被问过，就算没被问过的人，刷面试题的人也一定被问过。Yes，这里就要用一个，继承。

我们既然要用到ROS内部的MoveBase，就得遵守他的规则，也得遵守实现他的规则，我的理解就是得遵守它内部变量是咋定义的，以及它想从我这里调用的函数是咋定义的。那就有思路了，思路就是复制粘贴。。。。。。

/** include the libraries you need in your planner here */
 /** for global path planner interface */
 #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 <angles/angles.h>
 #include <base_local_planner/world_model.h>
 #include <base_local_planner/costmap_model.h>

 using std::string;

 #ifndef GLOBAL_PLANNER_CPP
 #define GLOBAL_PLANNER_CPP

 namespace global_planner {

 class GlobalPlanner : public nav_core::BaseGlobalPlanner {
 public:

  GlobalPlanner();
  GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /** overridden classes from interface nav_core::BaseGlobalPlanner **/
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan
               );
  };
 };
 #endif

全局路径规划器是用一个插件的东西加载到ROS系统的，我不懂啥是插件，但是看文章整体，应该是利用编译工具弄成了一个动态链接库后，想办法一顿猛如虎的操作告诉ROS咋引用我的东西。

解释一下上面的代码，头文件，Wiki原文有解释，可是以我当前学到的知识理解不了，所以忽略，反正头文件是干啥的大家都知道，变量类型定义，函数类型定义这些玩意，我们看具体的实现和具体的东西。

实现：一个命名空间+一个类。

命名空间以我那浅薄的C++知识理解，肯定是怕重名。类，就是规则想让我们要实现的东西，毕竟这就是类的定义，状态和行为么。。。如果把老板想成规则，那好，领导让干活，肯定要有一个交代和实现的基本要求，在ROS里的交代就是继承ROS已经实现好的一个类。

 public nav_core::BaseGlobalPlanner

然后在实现两个函数。。。。。 initialize 和 makePlan。好的好的。。。名字解释一切。

第二步：类的实例化

完成上面的东西，我们就把这个头文件交给老板，说我们这事干完了，领导扣工资都是轻的，把东西甩脸上估计是正确做法。因为这毕竟是老板讲话时候，咱们记录的提纲，后面举措还是要自己写。那么好的，老老实实的建一个cpp文件，不过好在这些举措不用马上实现，毕竟快过年了，我们可以在每个函数里写一个，老板新年快乐，送上一段美好的祝福吧。

#include <pluginlib/class_list_macros.h>
#include "global_planner.h"//这个文件就是咱们上面那个头文件，按照自己写的修改

 //register this planner as a BaseGlobalPlanner plugin
 PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

 using namespace std;

 //Default Constructor
 namespace global_planner {

 GlobalPlanner::GlobalPlanner (){

 }

 GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
   initialize(name, costmap_ros);
    
 }


 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    ROS_INFO("老板新年快乐");
 }

 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){

    plan.push_back(start);
   for (int i=0; i<20; i++){
     geometry_msgs::PoseStamped new_goal = goal;
     
    ROS_INFO("老板新年快乐");

   plan.push_back(new_goal);
   }
   plan.push_back(goal);
  return true;
 }
 };

其实这段代码也没啥解释的，但是，它的重要性，从名字上就看出来了，因为这里涉及到了给老板拜年的事情，哦不，是全局路径规划的事情。没错，makePlan，一个开头点坐标，一个目标点坐标，一堆（20个）中间点坐标，算出来，push_back进一个容器里面，好像就行了，全局路径就有了。那如何算？我目前也不知道，但是B站上一大堆的算法，什么A*、什么低接斯塔啦，估计就是那样算的吧。。。

第三步：修改Cmakelist和xml文件

这个没啥好说的，都写了那么多肯定是要编译的，不然和把这些东西写到纸上有啥区别。。。

CMakelist上面增加这个

add_library(global_planner_lib src/path_planner/global_planner/global_planner.cpp)

xml里面增加

<build_depend>nav_core</build_depend>
<run_depend>nav_core</run_depend>

然后，可以愉快的catkin_make了。

编译成功以后，可以去工作空间下面的devel/lib下面ls一下，就会看到编译的so文件出现了，可以动态加载了，只是名字前面增加了一个lib，想对于Cmakelist中我们添加那据话的地一个参数前面增加了一个lib，这个很关键，因为引用的时候，要告诉ROS系统，这个库叫啥，在哪。

第四步：引用插件的方法

老板交代的事情弄完了，接下来如何把成果告诉老板呢？直接交给漂亮的女秘书行不行，不行，因为一般女秘书不懂技术，得写个单子。那就写个递交单吧，递交单的格式就是新建一个xml文件，名字就写成global_planner_plugin.xml，然后写下面的东西：

尊敬的领导，2024年要开始了，2023年的年终奖。。。。。。

 <library path="lib/libglobal_planner_lib">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>This is a global planner plugin by iroboapp project.</description>
  </class>
 </library>

看到了吧，第一行就是插件编译生成的so的位置，相当于告诉老板，我交的那么多文件里面哪个才是全局规划器实现的。接下来class标签中的name是很关键的，那个就是全局规划器的名字，在launch文件中要被引用的！！！！！。

小秘书拿着这个承送单能不能直接送老板那里呢？指定不能啊，你得自己想好介绍的台词啊。那这个台词在哪里呢？再写个单子？没必要，写到我们自己的xml文件就行，有就是在最后的导出里面写这样一句：

<export>
<nav_core plugin="${prefix}/global_planner_plugin.xml" />
</export>

第五步：检查和调用

上面都写完了，就要检查调用了。方法就是写一个launch文件，注意，由于我们在xml文件中新增了一句话，要source一下才能把插件放到系统内部。可以通过下面的指令检查一下source之前和之后的区别，就知道增加了没有。检查的命令

rospack plugins --attrib=plugin nav_core

launch文件的格式是这个

 <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
 <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
</node>