老规矩,先来致谢,感谢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>
第六步:运行
运行相关的launch文件,注意要加入定位的相关功能包才行,估计打印的信息里面就可以看到给老板的新年祝福了。