本文共 4518 字，大约阅读时间需要 15 分钟。
4.2.1 MoveBaseActionServer 工作过程
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
转载地址：https://blog.csdn.net/lclfans1983/article/details/106730861 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！
发表评论
最新留言
关于作者
