其他分享
首页 > 其他分享> > ROS自定义路径规划器的原理及实现

ROS自定义路径规划器的原理及实现

作者:互联网

ROS自定义全局路径规划器原理及实现

虽然我还没有对象,但已不知面向了多少对象

——沃.兹基硕德

全局规划器的理论实现

上次写到宕开一笔去看move_base源码,死磕后也勉强是理解了,但是效率低效果差,属于是筋疲力尽且无效果。所以最开始如果想先看move_base的话,只需要了解一下类的继承关系和主要函数功能及执行顺序即可。

1. move_base实现流程

首先,move_base是什么?可以从几个不同的观点来看:

  1. 从最后结果来看,它是ROS中的一个节点:作为ROS中的一个节点,提供各种话题和服务;
  2. 从代码上来看,他是一个C++C类及其实例化:move_base功能的实现都是基于Movebase类的实例化来完成的,理解这个类的性质和功能也就理解了move_base的性能;
  3. 从实现原理来看:它是ROS框架下一个actionServer,在回调函数中执行各项供能;

因此对move_base的分析也可以从三点,实际上是两点即供能和实现。功能上篇已经分析过了,而实现则要从源码开始
该部分的源码大概分如下几部分:

大致来说就是以上几步,当然我做了一些简化省略了规则检查、坐标变换、抢占机制、和局部规划函数中繁杂的判断实现。如果想理解规划器的调用流程,理解到这里就够了,如果想深入了解:
ROS Navigation之move_base完全详解
源码分析系列
源码分析(侧重流程)

2. move_base与规划器联系

实现上述流程涉及到一些关键的类成员函数,具体如下:

1. 创建和初始化global planner
 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
 planner_ = bgp_loader_.createInstance(global_planner);

//使用的planner为 global_planner/GlobalPlanner
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);

2  创建和初始化local planner
 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
 tc_ = blp_loader_.createInstance(local_planner);
 
 //使用的planner为teb_local_planner/TebLocalPlannerROS
 tc_->initialize(blp_loader_.getName(local_planner), &tf_,  controller_costmap_ros_);
 4 创建并启动global planner线程
 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
 
 5 创建一个action server:MoveBaseActionServer。
 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base",    boost::bind(&MoveBase::executeCb, this, _1), false);

到现在我们大概已经理解了两个路径规划器在move_base中的调用流程,下一步就是理解规划器的具体功能和实现了;

3. Plugin和nav_core

ROS中采用插件机制来实现路径规划器功能,实现该功能有一套标准的流程:ROS中的插件plugin机制,该部分内容在此博客中有较为详细的描写,总的来说分为以下几点:

具体到move_base中,基类即nav_core,在该文章中有详细的介绍,该基类中的成员函数对应两个路径规划器的功能;然后基于该基类继承除了两种全局规划器:
global_planner: 一个快速的,内插值的路径规划器,其能更灵活地代替navfn;
navfn: 一个基于栅格的全局路径规划器,利用导航函数来计算路径;
该过程的实现与上文插件流程一样,具体见此:ROS: global_planner 整体解析

4. 两个路径规划器实现的功能

看完move_base源码在看nav_core这个基类,真的可以说是如天堂一样,没有线程、没有逻辑判断甚至没有具体实现,只有简简单单的功能描述,直接泪目啊喂。言归正传,nav_cove这个基类最主要的内容就是提供了插件功能的接口,换种说法来讲它定义了两个规划器和一个恢复插件的基本功能:

这篇文章主要分享全局规划器的实现流程,以GlobalPlanner规划器为例

5. 全局规划器的实现流程

功能类较基类相比内容要扩展很多,具体到GlobalPlanner规划器,其核心是planner_core类,除此之外还有一些辅助类,这些类相互继承、调用构成了很复杂的一套代码,具体简要介绍如下:

基于以上几个类来实现全局规划的功能,按照源码中的嵌套关系大概分为以下流程(以A*为例):

  1. 合法性检验和预处理,在planner_core.h定义的类中实现,同时也加载ros_param,并通过逻辑判断选择规划器、路径检索方式和路径拟合方式:
  2. 节点检索:在像素坐标系内开始搜索(每一个像素为一个开启点),输出一个px*py长度的一维数组,存放最后各个节点的开启状态。该部分在astar.h或者dijkstra.h定义的类(继承Expandar)内完成,入口函数为GlobalPlanner::makePlan函数中:
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                    nx * ny * 2, potential_array_);
  1. 路径搜索:调用栅格法或梯度下降法处理上一步中生成的数组,输出为一个数组,其元素为(float,float)分别存放路点的x和y坐标,该部分在两个搜索类中的一个完成(视参数决定),入口在GlobalPlanner::makePlan函数:
getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan))

//更为准确来说,是这个函数里面嵌套的:
path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)
  1. 路径发布:将路径转化为geometry_msgs::PoseStamped 格式后结束规划器;

全局规划参数和基本介绍
global_planner源码阅读笔记
基于A在navugation中添加自己的算法
浅谈路径规划算法(转载)
ROS中Navigation功能包里路径规划A算法详解

全局规划器的具体实现

基于之前的分析,我们大概能推断出全局规划器中各个类的调用顺序:
即先启动GlobalPlanner这个核心类;

在完成初始化后再该类内调用AStarExpansion或DijkstraExpansion这个类来实现节点检索的工能,这两个类继承Expander这个类,同时Expander类的拷贝构造函数初始化参数时传入PotentialCalculator这个类作为成员变量;

最后调用两个路径搜索类来实现节点列表数组到路点数组的转换;

各个类之间存在着极为复杂的继承与调用关系,因此如果想基于GlobalPlanner的源码进行更改,最好不要动函数结构比如增改传入参数之类的,建议还是按照原来的流程走哪怕你的算法并不需要这么多步骤;如果实在需要更改传入参数,建议做函数重载,这样不会打乱继承和调用的关系;

规划器更改心得

  1. 仿照Astar.h和Astar.cpp建立自己的规划方式类(其实算不上完整的规划,因为这个类只是在像素空间操作的),并更改Cmakelist;
  2. 在GlobalPlanner类初始化部分将规划器定向到自定义的规划器;
  3. 仿照GridPath类定义相应的路径搜索类,并更改CmakeList;
  4. 在GlobalPlanner类初始化部分将搜索器定向到自定义的;

照这个步骤做下来基本是没问题的,反正最起码我这是没问题的,能不能做成就看命运了,另外除了嵌套关系复杂外函数传参和各个类的全局变量定义也都不一样,所以强烈不建议大幅度更改,就做一个扩展就得了

另一种全局规划器的定义方法是参照carrot_planner,从头到尾按照ros::Plugin 的定义流程做,但这样做想避障还需要对Cost_map有深入了解
移动机器人——自定义规划算法

长大后,我们闭口不谈梦想

梦想很贵也很难,就像你对着一道题啃了一下午只收获了焦头烂额;
也像那个价格离谱到让你望而生畏的驱动板或者电机;
或是一次次很累但什么都没有做到的尝试。

后来我们不再谈及梦想,只谈近况;不想太远,只是一步一步向前走。

标签:move,planner,函数,自定义,路径,base,ROS,规划
来源: https://blog.csdn.net/dasifhoaisfg/article/details/122268591