move_base源码学习

综述

这个类调用actionlib::ActionServer接口接受目标，然后将小车导航到目标位置。
move_base状态机
movebase状态机分为planning，controlling，clearing 和default。
planning是指接受新目标与进行全局规划的整个过程，cotrolling是指调用局部规划器计算机器人速度的过程，clearing是指利用用户定义的行为进行recovery的过程。
状态机的关系图：

move_base类

构造函数MoveBase ()：
先定义了一个actionserver，收到目标之后调用executeCb。
调用ros参数服务器获得参数。设置三个plan（planner，controller，latest）的缓冲池。创建规划器线程planner_thread_，在该线程中运行planner_thread（）。
然后初始化global costmap和local cost map。初始化global/local planner。然后开始更新地图。然后声明一个makeplan的ros服务器、清除地图的服务器。最后启动actionserver。

executeCb（）：
首先调用tf将收到的目标变换到全局costmap下，然后锁定规划器互斥体，设定规划目标、然后启动规划器（改变规划器条件变量runplanner为true使规划线程中的makeplan开始工作），启动全局局部costmap，进入一个while循环。while循环中：根据actionserver状态不断接受新目标，并转换到全局costmap下，锁住规划器互斥体，给定规划目标，并发布该目标;判断全局costmap坐标系是否改变，并变换goalpose;执行executeCycle(goal, global_plan)。

executeCycle(goal, global_plan)：
首先锁住配置互斥体，获得机器人在全局中的位姿，actionserver发布该位姿。检查局部costmap是否是当前的，否则返回零速度。如果有新的全局规划，则将该规划传递至局部规划器tc_->setPlan(*controller_plan_)。然后根据move_base状态机的状态判断如何操作，switch判断movebse处于什么状态，规划状态下设置runplanner为true，控制状态下就调用局部规划器计算速度，清除状态下则尝试用户定义的恢复行为。

planner_thread（）：
锁住规划互斥体planner_mutex_，然后进入while循环。在while循环中：若若runplanner为假，则planner_cond_.wait(lock)，这里意思是解锁当前互斥体，此线程进入等待，等到别的线程里面出现cond.notify_one()后，此线程再次锁住互斥体。然后调用makeplan做全局规划。若得到全局规划的路径，然后将控制机状态设置为controlling。

makeplan（）
首先判断是否有地图，没有地图直接返回false，然后获得机器人的当前位姿，然后调用全局规划器插件的planner_->makePlan(start, goal, plan)进行全局规划。

void MoveBase::goalCB（）：publish 目标，更改来自action message的 PoseStamped的时间戳 and re-sending to the server。

planService（）：（rosservice的回调函数）
当确认actionserver被激活，并且costmap存在时，设置机器人的起点，更新costmapWindow，直接根据指定终点进行全局规划器规划planner_->makePlan（），若无法生成有效规划，则在该终点附近允许范围内一点作为目标进行planner_->makePlan（），将得到的一系列规划点返回到response中。

getRobotPose（）：
首先定义一个在机器人本体坐标系tf2::transform类型的位姿（其实就是原点），然后转换成geometry_msgs::Stampedpose的形式，然后利用tf2_ros::Buffer类的transform函数，将此位姿转换到全局坐标系下。

私有属性：

• 导航目标：actionserver，planner(指向全局规划器指针)，global_pose，planner_goal_
• 地图：planner_costmap_ros,controller_costmap_ros,各种radius
• 坐标系及变换：robot_base_frame,global_frame,tf
• 规划控制：planner_frequency_，controller_frequency_
• rosPublisher：current_goal_pub_，vel_pub_，action_goal_pub_
• rosSubscriber：goal_sub_（订阅rivz等软件发布的目标），
• ros::ServiceServer：make_plan_srv_，clear_costmaps_srv_
• 插件加载器：bgp_loader_（全局规划加载），blp_loader_（局部规划加载），recovery_loader_
• 规划结果：planner_plan_，latest_plan_，controller_plan_（都是vector类型）
• 线程：planner_mutex_（递归互斥体），planner_cond_（条件变量），planner_thread_（线程），configuration_mutex_（递归互斥体），
• bool量控制：shutdown_costmaps_，clearing_rotation_allowed_，recovery_behavior_enabled_，runPlanner_，new_global_plan_，p_freq_change_，c_freq_change_。