ROS Navigation源代码剖析(3)-move_base ActionServer工作流程

4.2.1 MoveBaseActionServer 工作过程

4.2.1.0完整流程

MoveBaseActionServer 的执行函数为：

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)；

输入参数为/move_base/goal [move_base_msgs/MoveBaseActionGoal]， 消息中包含了goalid一个字符串类型的变量， 和targetgoal，targetgoal中是目的地的坐标和朝向。具体消息内容可以参见4.2.1.3节。

4.2.1.0完整流程

整个的执行流程为：

1 位姿的四元数合法性检查

isQuaternionValid(move_base_goal->target_pose.pose.orientation)

检查的内容包括：

1） 四元数各个成员必须是有限量

2）四元数的长度不能为0

3）四元数的Z轴必须是垂直的

2 保存goal，通知global planner线程开始工作

    state_=PLANNING

    planner_goal_ = goal;

    runPlanner_ = true;

    planner_cond_.notify_one();

3 发布goal到topic /move_base/current_goal

current_goal_pub_.publish(goal);

4 进入循环，循环周期为controller_frequency每秒

4.1 检查是否有新的抢占式goal请求到达，

如果有，尝试获取新的goal

如果取到新的goal，则执行第2，第3步的内容

如果没有取到新goal，则重置状态，并退出这个流程。

4.2 检查goal的frameid与global planner的global frameid是否一样，

如果不一样，则进行TF转换，将goal转换为global frame并执行第2，第3步内容

下面的执行在函数MoveBase::executeCycle()中

4.3 从global costmap获取机器人当前位姿并发布feedback

4.4 计算当前位置与上次震荡位置的距离

如果大于设置的参数值，则修改正当位置为震荡位置

4.5 如果local costmap的数据过期，则停止机器人运动，并回到4.1重新开始循环

4.6 检查planner thread是否给出了路径规划

Bool变量new_global_plan_是action server线程与planner线程的通信器。

Planner线程计算出新的路径规划后，会将此变量设为true。

如果给出了路径规划（即new_global_plan_为true），则

4.6.1 将new_global_plan_置为false

用controller_plan_保存新的路径规划信息

4.6.2 将新的路径规划传给local planner

如果传递失败则重置状态，并退出整个action处理

4.6.3 重置recovery index

4.7 检查move_base的当前状态

如果是 PLANNING，则

runPlanner_ = true;

通知planner线程工作

如果是 CONTROLLING，则

1） 通过local planner检查是否已经到达目的地

如果已经到达目的地，则

重置状态

停止planner线程工作

发布MoveBaseResult成功

返回成功，并退出整个action处理

执行后面的步骤，说明还没有到达目的地

2） 检查是否处于震荡状态，如果是，则

停止运动

 state_ = CLEARING;

  recovery_trigger_ = OSCILLATION_R;

执行后面的步骤，说明还没有到达目的地

3） local planner计算需要的速度

如果计算成功，则

发布速度执行到topic /cmd_vel

清空recovery index

如果计算失败，则

如果已经超过了失败等待时间，

停止运动并进入recovery behavior

state_ = CLEARING;

recovery_trigger_ = CONTROLLING_R;

如果没有超过等待时间，

停止运动并回到步骤4.1重新规划路径

 state_ = PLANNING;

如果是CLEARING，则

如果开启了recovery behavior功能，则

执行recovery_index_对应的recovery行为

state_ = PLANNING，重新开始4.1循环，路径规划

recovery_index_++更新index， 下次会使用下一个recovery行为

如果没有开启recovery功能，则

停止planner线程工作

Actionserver返回失败result，重置状态，并退出整个action处理

至此，整个action server的处理过程完毕。

4.2.1.1订阅的topic

/move_base/goal [move_base_msgs/MoveBaseActionGoal]

4.2.1.2发布的topic

/move_base/goal [move_base_msgs/MoveBaseActionGoal]

/move_base/result [move_base_msgs/MoveBaseActionResult]

/move_base/feedback [move_base_msgs/MoveBaseActionFeedback]

/move_base/status [actionlib_msgs/GoalStatusArray]

4.2.1.3具体的消息格式

/move_base/goal

rosmsg show move_base_msgs/MoveBaseActionGoal

std_msgs/Header header

uint32 seq

time stamp

string frame_id

actionlib_msgs/GoalID goal_id

time stamp

string id

move_base_msgs/MoveBaseGoal goal

geometry_msgs/PoseStamped target_pose

std_msgs/Header header

  uint32 seq

  time stamp

  string frame_id

geometry_msgs/Pose pose

  geometry_msgs/Point position

    float64 x

    float64 y

    float64 z

  geometry_msgs/Quaternion orientation

    float64 x

    float64 y

    float64 z

    float64 w

/move_base/result

[move_base_msgs/MoveBaseActionResult]

rosmsg show move_base_msgs/MoveBaseActionResult

std_msgs/Header header

uint32 seq

time stamp

string frame_id

actionlib_msgs/GoalStatus status

uint8 PENDING=0

uint8 ACTIVE=1

uint8 PREEMPTED=2

uint8 SUCCEEDED=3

uint8 ABORTED=4

uint8 REJECTED=5

uint8 PREEMPTING=6

uint8 RECALLING=7

uint8 RECALLED=8

uint8 LOST=9

actionlib_msgs/GoalID goal_id

time stamp

string id

uint8 status

string text

move_base_msgs/MoveBaseResult result

/move_base/feedback

[move_base_msgs/MoveBaseActionFeedback]

rosmsg show move_base_msgs/MoveBaseActionFeedback

std_msgs/Header header

uint32 seq

time stamp

string frame_id

actionlib_msgs/GoalStatus status

uint8 PENDING=0

uint8 ACTIVE=1

uint8 PREEMPTED=2

uint8 SUCCEEDED=3

uint8 ABORTED=4

uint8 REJECTED=5

uint8 PREEMPTING=6

uint8 RECALLING=7

uint8 RECALLED=8

uint8 LOST=9

actionlib_msgs/GoalID goal_id

time stamp

string id

uint8 status

string text

move_base_msgs/MoveBaseFeedback feedback

geometry_msgs/PoseStamped base_position

std_msgs/Header header

  uint32 seq

  time stamp

  string frame_id

geometry_msgs/Pose pose

  geometry_msgs/Point position

    float64 x

    float64 y

    float64 z

  geometry_msgs/Quaternion orientation

    float64 x

    float64 y

    float64 z

    float64 w

/move_base/status

[actionlib_msgs/GoalStatusArray]

rosmsg show actionlib_msgs/GoalStatusArray

std_msgs/Header header

uint32 seq

time stamp

string frame_id

actionlib_msgs/GoalStatus[] status_list

uint8 PENDING=0

uint8 ACTIVE=1

uint8 PREEMPTED=2

uint8 SUCCEEDED=3

uint8 ABORTED=4

uint8 REJECTED=5

uint8 PREEMPTING=6

uint8 RECALLING=7

uint8 RECALLED=8

uint8 LOST=9

actionlib_msgs/GoalID goal_id

time stamp

string id

uint8 status

string text