move_base源码学习

综述

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

move_base类

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

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

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

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

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

void MoveBase::goalCB&＃xff08;&＃xff09;&＃xff1a;publish 目标&＃xff0c;更改来自action message的 PoseStamped的时间戳 and re-sending to the server。

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

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

私有属性&＃xff1a;

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