move_base源码学习

综述

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

规划一次后自动进入
规划尝试次数过多
最新避障时刻太旧
planning
controlling
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_。

猜你喜欢

转载自blog.csdn.net/qq_41986495/article/details/85228745