movebase导航和地图数据的使用 - 4 movebase 如何处理target请求

1. 第三方向/move_base_simple/goaltopic发送消息,消息包含一个新的目标=》MoveBase::goalCB函数响应。

2. MoveBase::goalCB向/move_base/goal转发消息。

我们有理由相信。向/move_base/goal发送的消息会导致actionserver的相应callback被调用

因为movebase建立了一个actionServer:MoveBaseActionServer, (其实是SimpleActionServer)而该server的初始化代码（action_server_imp.h中的voidActionServer<ActionSpec>::initialize()）函数中可以发现，该actionServer自己会监听”goal”topic,上的事件，当有新的goal来到时，会调用内部的callback： SimpleActionServer<ActionSpec>::goalCallback，它设置标记有了新的goal,然后其内部的SimpleActionServer<ActionSpec>::executeLoop()循环在检查到有新的goal后，才会调用我们传递给该server的用户callback函数execute_callback_(goal);新设定的goal会导致SimpleActionServer<ActionSpec>::executeLoop里面调用MoveBase::executeCb。且是另一个不同于node的线程调用该callback.而且该线程自己是一个loop,不断检查是否有新的goal被设置，如果被设置就调用该callback.

3.MoveBase::executeCb（move_base_goal）被触发，运行于一个actionServer建立的独立线程里

这个线程是SimpleActionServer<ActionSpec>::SimpleActionServer里面建立的，simple_action_server_imp.h中，

execute_thread_ = newboost::thread(boost::bind(&SimpleActionServer::executeLoop,this));

将move_base_goal坐标变幻成global坐标，并保存到planner_goal_。

通知planner_cond_信号量，唤醒planthread，告诉它新来了一个goal,需要从新进行plan的计算。

具体如何实现newgoal覆盖老的goal的逻辑我没有时间看了。

但是可以确定功能上的执行逻辑顺序。

4. MoveBase::planThread被唤醒，根据得到的新的goalplanner_goal_调用makePlan得到一个新的plan,并保存到planner_plan_里。然后更新到latest_plan_里。并将状态更新成CONTROLLING。

这里调用makeplan得到一个新的plan实际上是调用globalplanner的makePlan.Global planner是navfn/NavfnROS。

5.MoveBase::executeCb继续执行，调用executeCycle作具体的驱动底座的工作。注意，executeCb运行于actionserver建立的一个独立线程里面，所以底座的具体驱动工作，又一个独立于movebase主线程的actionserver线程执行。

6. bool MoveBase::executeCycle(geometry_msgs::PoseStamped&goal, std::vector<geometry_msgs::PoseStamped>&global_plan)工作如下：

先得到当前的robot位置，然后以feedback方式发送出去，通知actionServer的使用者。

如果发现拿到一个新的plan,就调用localplanner的setPlan设置给localplanner。

然后判断当前的状态，如果是CONTROLLING，那么就判断是否已经到了终点，如果没有到，就让localplanner计算速度命令：computeVelocityCommands()，然后发送给cmd_veltopic. Local planner是DWAPlannerROS。

Note:NavfnRos和DWAPlannerRos分别继承自BaseGlobalPlanner和BaseLocalPlanner,实现了父类接口，并且以plugin的形式通过classloader加载，使得他们可以方便的被替换。他们分别封装了NavFn和DWAPlanner。