Move_base_flex(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 nav的更新在于将脱困，控制，规划整成独立模块，相较于ros1的move_base规划控制脱困相互耦合更容易做处理和拓展，缺点是没有高度抽象和高度复用性。

2.2 mbf类图主体思路详解

重点声明
以下思路分析全部基于抽象层 即图中对应的 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解析

1. 当在rviz上给了一个目标点goal之后，对应的topic回调就会调用/move_base action
2. 收到move_base的client唤起后，costmap_navigation_server (抽象导航类的实现-子类) 就会调用对应的执行。
3. 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模块举例

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

7. 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实时运行算法。

5. 基于mbf框架实现导航代码推荐

1. mesh_navigation(mbf公司实现的3D导航算法，用于崎岖地形):https://github.com/uos/mesh_navigation

2. xju机器人(国人制作)，内含各种室内清扫机器人入门干货，墙裂推荐:https://github.com/Mr-Tony921/xju-robot

3. FCPP全覆盖清扫全局规划算法，使用了mbf框架进行执行清扫:https://github.com/nobleo/full_coverage_path_planner

4. 官方教程：https://uos.github.io/mbf_docs/tutorials/beginner/basic_navigation/