Move_base_flex(mbf)框架理解

最新推荐文章于 2024-05-27 16:14:39 发布

ElenHuo

最新推荐文章于 2024-05-27 16:14:39 发布

阅读量3.5k

点赞数 6

分类专栏： ROS 路劲规划文章标签： c++ ROS move_base 规划导航架构

本文链接：https://blog.csdn.net/GeForeverr/article/details/128593447

版权

ROS 同时被 2 个专栏收录

12 篇文章 2 订阅

订阅专栏

路劲规划

1 篇文章 0 订阅

订阅专栏

本文章重点在 第二部分类图解析，第四部分代码解析

文章目录

1. move_base_flex主体代码结构树
2. move_base_flex 类图解析
3. mbf向后兼容move_base解析
4. mbf重点代码详解
5. 基于mbf框架实现导航代码推荐

1. move_base_flex主体代码结构树


├── mbf_abstract_core (抽象父类，提供控制-局部规划，全局规划，脱困行为基础功能接口)
├── mbf_abstract_nav (抽象导航类，提供控制，规划，脱困执行行为虚函数接口和action对应的状态机以及插件管理器）
├── mbf_costmap_core (子类，基于代价地图costmap实现父类abstract_core的功能接口)
├── mbf_costmap_nav (子类，基于代价地图实现导航功能的接口的设置，action初始化，各接口执行器Execution算法绑定）
├── mbf_msgs (提供局部规划，全局规划，脱困，点到点move_base的action，以及检查路径点代价值等srv)
├── mbf_simple_nav (基于mbf实现简单的导航服务，但是局限性在于没有地图描述。可用于对costmap_nav简化剥离)
└── mbf_utility (mbf框架工具箱，包含速度，里程计获取,坐标变换等)

总结： 这里面的costmap_core，costmap_nav，simple_nav都是属于子类的实现，父类已经把整体流转框架思维写好了。
后续结合mbf类图的参照，就可以实现基于各种不同地图类型的2D，3D导航(如mbf框架已实现的costmap导航，和mesh_gridmap的3D导航)。总结来说，该框架重点在于mbf_abstract_core, mbf_abstract_nav, mbf_msgs, mbf_utility

2. move_base_flex 类图解析

总结：move_base_flex的整体框架思维与ROS2 navigation具备高度相似。将控制，规划，脱困分模块解耦独立工作。
ROS2 navROS2 nav通过行为树发送对应的action，让各个模块做自己对应的工作。
mbf则通过navigation_server去初始化，加载各个模块。本质上也是通过上层如行为树，状态机去调用对应的
action实现运转。因此mbf与ROS2 nav架构思想十分类似，区别在于mbf将父类抽象剥离，只要重新写子类的实现，对应的地图类型就能够使用，如mesh_navigation。具有再编辑性，复用性，可移植性

2.1 ROS2 navigation整体架构

ROS2 navigation包的整体架构图
ROS2 nav的更新在于将脱困，控制，规划整成独立模块，相较于ros1的move_base规划控制脱困相互耦合更容易做处理和拓展，缺点是没有高度抽象和高度复用性。

2.2 mbf类图主体思路详解

move_base_flex框架总体概括图

重点声明
以下思路分析全部基于抽象层即图中对应的 Abstract Move Base Flex Level 层，从左到右分析！！！！！！！！！！！！！！！！！！！！！！！！！！！！

2.2.1. 抽象层（abstract层）

abstract层有规划，控制，脱困执行器（abstract Execution），通过插件加载的方式去调用其对应的算法。每个Execution都有对应的独立线程单独运行里面的各个状态机。 navigation server类似大脑，通过接收tf树，外部数据输入，然后再接收外部的action信号（get_path，exe_path, recovery），直接开启各模块action server内部的流转，运行起了各模块Execution。

2.2.2. 外部信号输入（Action层）

在Abstract Navigation Server中，先注册好各个模块的action server等待外部的 client去唤起对应的行为。如外部client唤起了get_path的action，就会执行器Abstract Planner Execution的流程，进入其状态机流转，生成对应的全局路径，并返回到action 的result中去 （该生成路径可以用于后续的exe_path中调用控制模块）
再比如外部client唤起了exe_path的action，会让控制模块独立运行，并一直反馈当前路径执行状态和最后执行结果。
通过Action Server的外部调用，就能实现各个模块的解耦。

2.2.3. 外部API输入

外部节点node(写好了服务srv端/make_plan)，然后client 一旦唤起，srv回调中就会去get_path对应的client就会唤起对应的action server进行全局路径规划。同理，topic也是，节点接收到对应的topic，回调就会action client call对应的server，进行调用move_base Action，进行其对应的流转。

3. mbf向后兼容move_base解析

当在rviz上给了一个目标点goal之后，对应的topic回调就会调用/move_base action
收到move_base的client唤起后，costmap_navigation_server (抽象导航类的实现-子类) 就会调用对应的执行。
move_base_action_.start(goal_handle)为主要实现函数，里面分顺序先调用全局规划action_client_get_path_.sendGoal，生成全局路径后，在全局路径action结果返回的状态succeeded中向控制模块发出路径的action server唤起指令action_client_exe_path_.sendGoal。该action里面包含了actionExePathDone,actionExePathFeedback. 主要用于执行结果以及执行过程中的状态。当机器人处于oscillation时，actionExePathFeedback中会调用attemptRecovery()函数，函数里面会唤起脱困的action，对应Recovery的行为。

4. mbf重点代码详解

重点声明以下思路分析全部基于抽象层
本章节拿controller模块举例

abstract_navigation_server里面已经注册好了controller模块的算法，以及对应的action服务器，为action_server_exe_path_ptr_.
当收到路径点时候就会进入回调函数callActionExePath
callActionExePath先判断有没有指定要求哪个控制算法，然后通过插件管理器加载对应算法
创建并赋值controller_execution指针，接着controller_action_.start(goal_handle, controller_execution)实现控制模块的执行行为线程入口。
若控制模块没有被新的路径进来中断，则子类特意单独执行父类的AbstractActionBase::start(goal_handle, execution_ptr);
在父类中开启了关于控制模块独立运行的线程threads_.create_thread(boost::bind(&AbstractActionBase::run, this, boost::ref(concurrency_slots_[slot]))); run函数里面包含了runImpl，优先调用的是子类的实现，由于入口最开始是从子类开始的。即调用了ControllerAction::runImpl函数。因此控制模块的线程循环关键函数为ControllerAction::runImpl函数。

case AbstractControllerExecution::INITIALIZED:
 	execution.setNewPlan(plan, goal.tolerance_from_action, goal.dist_tolerance, goal.angle_tolerance);
 	execution.start();
上面代码极其重要，因为调用了runImpl后，控制模块的Execution处于初始化状态，会先设好要跟随的路径以及execution.start();创建了一个新进程同步运行控制的Execution，
即控制算法执行在其他进程中一直进行。因此runImpl函数属于将控制算法跟踪的状态，Action的结果实时反馈到/exe_path中，即publishExePathFeedback()函数和fillExePathResult()

总结：有两个线程独立运行，一个为ControllerAction线程反馈Action状态，一个ControllerExecution实时运行算法。