前言
- 在ROS运动规划学习一—navigation整体框架中提到，move_base功能包整合了导航的全局路径规划、局部路径规划以及恢复行为模块，实现机器人的导航，这里介绍下move_base模块。
一、move_base工程结构
move_base结构如下所示：
其中move_base.h定义了MoveBase类，该类是实现路径规划与导航的类。
二、move_base简要分析
1.MoveBase类的声明—move_base.h
首先重定义Actionlib的服务 MoveBaseActionServer，并声明MoveBaseState的联合体，表示当前的movebase状态，以及恢复行为原因的联合体，表示引起恢复行为的原因。
enum MoveBaseState {
PLANNING, //正处于规划路径
CONTROLLING, // 正在控制运动
CLEARING // 处于恢复或者清除状态，规划失败了或者控制运动失败
};
enum RecoveryTrigger
{
PLANNING_R, // 全局规划失败
CONTROLLING_R, // 局部轨迹规划失败
OSCILLATION_R // 长时间小范围运动
};
MoveBase类的成员函数：
构造函数, action构造函数：tf变换的引用
MoveBase(tf2_ros::Buffer& tf);
成员函数：executeCycle，机器人导航到目标点的控制函数，返回值True到达，否则返回false
bool executeCycle(geometry_msgs::PoseStamped& goal);
clearCostmapsService：代价地图清除服务
bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
MoveBase类的makePlan成员函数，注意与nav_core中的进行区分，goal目标点，plan规划的路径，返回True表示规划成功
bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
loadRecoveryBehaviors：加载恢复行为函数，返回True表示成功的加载恢复行为
bool loadRecoveryBehaviors(ros::NodeHandle node);
loadDefaultRecoveryBehaviors：加载默认恢复行为
void loadDefaultRecoveryBehaviors();
clearCostmapWindows：清楚机器人周围窗口内的障碍物。size_x,size_y窗口尺寸大小，和参数clearing_radius有关
void clearCostmapWindows(double size_x, double size_y);
publishZeroVelocity：发布零速度函数
resetState：恢复movebase状态,同时发布零速度，禁止机器人运动
void publishZeroVelocity();
void resetState();
goalCB：接收目标回调函数；
planThread：开辟全局规划线程，进行全局路径规划；
executeCb控制的主要函数，控制机器人底盘的重要函数；
isQuaternionValid：四元数检查函数，检查四元数是否合法，以及长度是否趋于零，并归一化四元数，不合法返回false。
getRobotPose：得到机器人位置
distance：机器人运动距离计算，和参数oscillation_distance比较
goalToGlobalFrame：将目标点转换到全局坐标系
void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
void planThread();
void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
bool isQuaternionValid(const geometry_msgs::Quaternion& q);
bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap);
double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
wakePlanner：定时器唤醒
void wakePlanner(const ros::TimerEvent& event);
MoveBase类的成员变量：
之后定义了路径规划以及恢复行为执行过程的一些成员变量、发布接收者、服务等。其中tf_是TF坐标系的变换，as_是导航的服务，actionserver的服务器；
tf2_ros::Buffer& tf_;
MoveBaseActionServer* as_;
tc_ 局部路径规划器实例化后的指针，指向local_planner；代价地图指针：planner_costmap_ros_指向全局代价地图，controller_costmap_ros_指向局部代价地图，planner_全局规划器指针；recovery_behaviors_恢复行为vector，存储全部的恢复行为，一般默认是转圈。这里就和ROS运动规划学习二—nav_core中的nav_core模块对应上。
boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
std::string robot_base_frame_, global_frame_;
std::vector<boost::shared_ptr<nav_core::RecoveryBehavior>> recovery_behaviors_;
std::vector<std::string> recovery_behavior_names_;
unsigned int recovery_index_; // 恢复行为索引
然后以插件的形式实现全局规划器、局部规划器以及恢复行为。在已经注册插件的基础上，插件形式可以实现动态的加载C++类。
//全局规划器
pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
//局部规划器
pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
//恢复行为规划器
pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
保存规划器中刚刚算出的路径，planner_plan_ 传给latest_plan_ ，作为一个桥梁，在executeCycle函数中传递给controller_plan_
std::vector<geometry_msgs::PoseStamped>* planner_plan_;
std::vector<geometry_msgs::PoseStamped>* latest_plan_;
std::vector<geometry_msgs::PoseStamped>* controller_plan_;
还有其他成员变量，比如用于动态参数服务器的等等。
2.MoveBase类的实现—move_base.cpp
MoveBase实现：构造函数初始化列表，成员变量进行初始化操作：
MoveBase::MoveBase(tf2_ros::Buffer& tf) :
tf_(tf),
as_(NULL),
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
......
}
actionserver实例化一个对象，一直执行回调函数executeCb。
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
并加载了move_base的全局参数、make_plan、costmap的参数，和yaml文件中的类似，若yaml文件中没有设置则默认使用这里的参数。
std::string global_planner, local_planner;
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1);
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
新建planner线程，入口函数planThread；
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
vel_pub_速度下发命令，将速度发送给机器人底盘；current_goal_pub_当前目标点发布指令，发布目标点。
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
动作行为发布，目标点以及恢复行为的信息；
ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);
订阅rviz下发的move_base_simple中的goal话题，获取目标点
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
之后开始定义并初始化全局规划器、局部规划器。
try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}
try {
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
发布的服务—make_plan，清除代价地图服务clearCostmapsService分别清楚全局代价地图和局部代价地图
//advertise a service for getting a plan
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
//advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
若加载恢复行为没有参数，加载默认的恢复行为，实现在loadDefaultRecoveryBehaviors函数中，加载对应的恢复行为的插件，清除代价地图的恢复行为、原地旋转的恢复行为、主动积极地代价地图重置行为，以及再次原地旋转，没有加载成功出现错误警告。
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}
初始化结束后，actionserver启动,开始进行路径规划。
void MoveBase::planThread() 调用全局路径规划获取路径，保证运动规划的周期性。因为是另外一个线程，要加锁，首先确认是否要运行全局路径规划器，若需要唤醒或者局部规划器没有运行，则等待。
开始进行规划：获取目标点planner_goal，并赋值给temp_goal；因为在上次循环中加锁，这次解锁。
运行路径规划器，它的主要函数是makePlan，该函数中首先清空路径，判断是否有全局代价地图，能否获取机器人的起始位姿，均满足则调用全局规划器的makePlan函数，也就是继承nav_core中基类的全局规划器，进行规划，若规划器失败或者规划路径为空，则表明规划失败，返回fasle,以上均成功返回True。
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
若规划器规划出路径则更新相应的全局路径，将planner_plan_放到latest_plan_中，若没有到达目标点，将move_base状态置为 CONTROLLING状态，为了之后的局部路径规划，同时这里会有线程保护。若没有规划出全局路径，同时move_base状态为PLANNING（在executeCycle或者重置状态时设置，一般是刚获得新目标，或者得到路径但计算不出下一步控制，重新进行规划），进入障碍物清楚状态，发布零速度publishZeroVelocity()，恢复行为的原因为PLANNING_R。
若还没达到规划周期则定时器睡眠，在定时器休眠wakePlanner()中通过planner_cond_唤醒。
//规划频率大于零，还没达到下一周期时，休眠时间大于零，需要等待，令wait_for_wake = true,在planThread线程中等待被唤醒
if(planner_frequency_ > 0){
ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
if (sleep_time > ros::Duration(0.0)){
wait_for_wake = true;
//以固定时间对回调函数进行调用
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}
以上就是planThread()线程的分析，全局路径规划完成以后，进行局部规划的控制，主要执行函数是MoveBase::executeCb（）
void MoveBase::executeCb 程序运行后，回调函数一直刷新，进行局部规划控制，首先判断目标点朝向的四元数是否合法，判断目标点是否有效，并统一转换到全局坐标系 ；发布零速度，有了目标点后开始进行路径规划。加锁，将目标点goal给到全局路径规划planner_goal,令 全局路径标志位 runPlanner_ = true，由于全局规划器线程planThread()有planner_cond_对象的wait函数，这里调用notify唤醒启动全局规划器线程，进行全局规划，并解锁。全局规划完成，就开始进行局部规划，完成导航任务。
if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
publishZeroVelocity();
局部规划：判断action_server是否被占用，占用有两种情况：
- 1 局部规划过程有了新的目标点，如果有了新的目标点，则执行新的目标，把之前的过程重新走一遍：首先判断目标点朝向的四元数是否合法，判断目标点是否有效，并统一转换到全局坐标系，加锁，将目标点goal给到全局路径规划planner_goal,令 全局路径标志位 runPlanner_ = true，由于全局规划器线程planThread()有planner_cond_对象的wait函数，这里调用notify唤醒启动全局规划器线程，进行全局规划，并解锁。然后发布目标点给RVIZ。
if(as_->isNewGoalAvailable()){
move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}
goal = goalToGlobalFrame(new_goal.target_pose);
recovery_index_ = 0;
state_ = PLANNING;
lock.lock();
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
current_goal_pub_.publish(goal);
// 相关的计时变量也要更新
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
}
- 2 收到取消任务的命令。重置导航各个部分的状态，执行resetState()函数，把当前的目标点设为占用状态。
若没有被占用或之前已经执行完毕，则开始进行局部规划。先确认目标点坐标系是否和全局坐标系一致，进入planning状态，准备下个执行周期。确认路径规划是唤醒的，发布目标点到RVIZ，相关计时变量更新，也就是和之前的操作差不多。之后，调用executeCycle函数，记录局部控制起始时间（真实时间），导航到目标位置的主要执行函数。
executeCycle() 函数：加锁，定义cmd_vel发布速度命令，获取机器人当前位姿，并发布机器人当前位姿；判断机器人走了足够的距离，重置震荡时间 ，若上次的恢复行为由震荡导致的，重置恢复索引。
move_base状态机切换，控制导航：
- PLANNING状态，则加锁，唤醒全局路径规划器线程；
- CONTROLLING状态：判断是否到目标点，若到达，重置导航各部分状态，执行resetState()，并关闭路径规划线程；没有到达，进行震荡判断，防止卡在一定区域，若产生振荡，则发布零速度，状态置为CLEARING，产生恢复行为原因置为OSCILLATION_R；若没有震荡，开始进行局部规划，调用局部规划器的成员函数computeVelocityCommands()，计算出速度，下发给机器人底盘。若局部规划失败，先判断控制命令是否超时，超时，发布零速度，进行障碍物清除恢复行为（move_base状态为CLEARING，产生恢复行为原因为 CONTROLLING_R）；没有超时，但找不到全局路径，重新启动全局路径规划器线程重新规划。
- CLEARING状态：用提供的恢复行为来清理空间，主要有三个恢复行为，分别调用各自恢复行为的成员函数。若没有恢复成功，则关闭全局规划器结点，打印错误。
总结
本文学习了move_base中的主要函数，用于全局规划线程的函数planThread，局部规划函数executeCb、executeCycle，其它的函数：重置函数resetState、获取机器人位姿函数getRobotPose、计算距离函数distance、目标点转全局坐标函数goalToGlobalFrame、唤醒规划函数wakePlanner 四元数检查函数isQuaternionValid、零速度发布函数publishZeroVelocity等函数作为辅助函数，这里并没有仔细分析。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
