本文章重点在 第二部分类图解析,第四部分代码解析
文章目录
- 1. move_base_flex主体代码结构树
- 2. move_base_flex 类图解析
- 2.1 ROS2 navigation整体架构
- 2.2 mbf类图主体思路详解
- 2.2.1. 抽象层(abstract层)
- 2.2.2. 外部信号输入(Action层)
- 2.2.3. 外部API输入
- 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 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解析
- 当在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实时运行算法。
5. 基于mbf框架实现导航代码推荐
mesh_navigation(mbf公司实现的3D导航算法,用于崎岖地形):
https://github.com/uos/mesh_navigationxju机器人(国人制作),内含各种室内清扫机器人入门干货,墙裂推荐:
https://github.com/Mr-Tony921/xju-robotFCPP全覆盖清扫全局规划算法,使用了mbf框架进行执行清扫:
https://github.com/nobleo/full_coverage_path_planner官方教程:
https://uos.github.io/mbf_docs/tutorials/beginner/basic_navigation/