ROS源码学习move_base(1)——导航流程

概述

move_base是一个开源2D移动机器人导航包,实现机器人自主导航到给定目标点的功能。move_base提供配置、运行以及跟导航栈交互的接口,他的基本框架如下图所示,其核心包含了3大功能模块:nav_core::BaseGlobalPlannernav_core::BaseLocalPlannernav_core::RecoveryBehavior,他们以插件的形式加载到move_base中,分别实现全局路径规划、局部路径规划和恢复机制。

move_base基本框架

源码流程整理

move_base导航包就一个类,在move_base_node中初始化化节点后,实例化move_base::MoveBase类后,便进入导航流程。

初始化

初始化包括初始化类成员变量,从参数服务器中加载参数,启动线程planThread控制导航的主线,注册话题、服务、动态参数服务器,订阅话题以及加载全局路径规划、局部路径规划和恢复机制等模块。可以看到,在初始化后,程序在planThread线程中利用planner_cond_.wait(lock)进行阻塞,一直等待planner_cond_.notify_one()的唤醒。而唤醒主要发生在机器人接收到新目标调用executeCb函数中,在唤醒planThread的流程后,executeCb则开始执行导航控制流程executeCycle,在executeCycle中主要是利用状态机根据不同条件切换机器人当前状态,总共包括了3种状态:PLANNINGCONTROLLINGCLEARING,并且在相应的状态下执行相应的动作。

planThread

当线程planThread的阻塞被唤醒后,开始进行全局规划路径,执行makePlan,如果成功规划出路径,则更新一些变量,同时将机器人状态切换至CONTROLLING;如果未能规划出全局路径,则循环进行重试,若重试至超时或者次数超出规定次数后,planThread则重新进入阻塞。直到定时器定时唤醒或者接收到新目标才又唤醒。planThread流程如下图所示。

planThread运行流程

executeCB

executeCBmove_base接收到新目标时调用的函数,该函数主要实现的是执行executeCycle前的检测和完成一些变换,包括了多四元数的合法性判断、目标点向全局坐标系的转换以及action服务器目标抢占的判断和设置。对于抢占的理解参考这里中的7. 【SimpleActionServer Goal Policies】,具体流程如下图所示。

executeCB运行流程

executeCycle

executeCycle主要是开始对机器人进行控制,将全局路径设置到控制器,由局部路径规划器计算机器人的运动速度和方向,并发布cmd_vel话题。在这里也实现主要的状态机状态转换逻辑,控制流程如下图所示。

executeCycle运行流程

状态机状态转换图

总结

move_base将导航流程规范化并且进行了封装,将整过导航过程总结为全局路径规划、局部路径规划和恢复机制,并且以插件的方式加载这3个模块,在使用中我们可以基于move_base这个框架定制自己的全局路径规划器、局部路径规划器和恢复机制。

参考

0%