文章目录

前言
一、move_base工程结构
二、move_base简要分析
- 1.MoveBase类的声明---move_base.h
- 2.MoveBase类的实现---move_base.cpp
总结

前言

在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等函数作为辅助函数，这里并没有仔细分析。

ROS运动规划学习三---move_base相关推荐

ROS运动规划学习六---dwa_local_planner
文章目录前言一.dwa_local_planner结构二.setPlan.initialize.isGoalReached 三.computeVelocityCommands() 总结前言在 ...
ROS运动规划学习五---global_planner
文章目录前言一.global_planner功能包结构二.planner_core 1.执行过程 2. calculatePotentials() 3. getPlanFromPotential ...
运动规划学习笔记4——探索OMPL
运动规划学习笔记4--探索OMPL A.OMPL编译与安装 B.OMPL使用 B1.基本定义 B2.路径可视化 B3.API Overview C.代码附录 C1.Geometric Planning ...
rk3288 img打包工具_【个人开源】机器人运动规划学习工具箱使用说明
最近的课题与机器人的运动规划有关.回顾过去的学习经历,深感机械出身的我们编程实践与总结能力实在太弱,以前的一些课题就拿matlab随便写一写m文件出个结果了事,许多后来发现有用的课程或作业,要么找不到 ...
深蓝学院-机器人运动规划学习笔记-第一章
第一课移动机器人运动规划 Motion planning for mobile robots Introduction Course outline Typical planning methods ...
无人机核心技术之运动规划
转载自:https://mp.weixin.qq.com/s/VHaqsfNhl9yStMX--Gtx6g 无人机核心技术之运动规划奕木怒飞垂云 1周前近年来无人机已广泛落地应用在农业.军工甚至 ...
ROS机器人操作系统学习笔记（三）ROS通信架构
ROS机器人操作系统学习笔记(三)ROS通信架构 ROS的通信架构是ROS的灵魂,也是整个ROS正常运行的关键所在.ROS通信架构包括各种数据的处理,进程的运行,消息的传递等等.本章主要介绍了通信架构 ...
【论文笔记】基于强化学习的机器人手臂仿人运动规划方法
文章目录摘要关键词 0 引言学者研究阶段一:采集运动数据,分析运动过程特征阶段二:设计仿人变量.建立仿人标准和约束阶段三:用智能算法提升仿人运动机器人性能本文工作 1 问题描述及方法架构 ...
【实战+源码】RGB-D移动抓取服务机器人（四）——完结篇（ROS机器人、系统设计、运动规划、目标定位）
毕业设计已经完成三个多月了,四月底答辩结束,上周办完离校手续!善始善终,最后一篇结束把这个题目告一段落! 完整代码github托管地址:https://github.com/pengxinyi-up/ ...

ROS运动规划学习三---move_base