机器人规划算法——movebase导航框架源码分析

在这里插入图片描述

这里对MoveBase类的类成员进行了声明，以下为比较重要的几个类成员函数。

构造函数 MoveBase::MoveBase | 初始化Action 控制主体 MoveBase::executeCb
收到目标，触发全局规划线程，循环执行局部规划全局规划线程 void MoveBase::planThread | 调用全局规划
全局规划 MoveBase::makePlan | 调用全局规划器类方法，得到全局规划路线局部规划
MoveBase::executeCycle | 传入全局路线，调用局部规划器类方法，得到速度控制指令

1.MoveBase的构造函数

以下是MoveBase的构造函数，为了方便本文的写作，我将它的语句顺序做了适当的调整，不影响整个系统的运行。如下面的代码片段所示，在一开始对一系列的成员变量赋予了初值：
tf_是坐标变换TF2的接口对象;
as_则是Action服务器;
planner_costmap_ros_是用于全局规划器的代价地图对象;
controller_costmap_ros_则是局部规划器所用的代价地图对象;
bgp_loader_是装载全局规划器插件的工具;
blp_loader_是装载局部规划器插件的工具;
planner_plan_, latest_plan_ 和controller_plan_是三个记录规划结果的缓存;
runPlanner_, setup_, p_freq_change_, c_freq_change_, new_global_plan_则是一些控制和反映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)

在构造函数的一开始，定义了两个ROS的句柄，用于获取节点参数，订阅和发布主题。以下面的第13和14行为例，MoveBase从参数服务器中获取了全局规划器和局部规划器的名称，如果系统中没有定义这些参数，将以默认值"navfn/NavfnROS"和"base_local_planner/TrajectoryPlannerROS"完成初始化工作。还有很多其它参数需要配置，这里不再一一介绍。

			ros::NodeHandle private_nh("~");
            ros::NodeHandle nh;
            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"));
            // 省略其它加载参数的语句

然后，构建了轨迹规划结果缓存planner_plan_, latest_plan_和controller_plan_。并且创建了一个线程planner_thread_用于轨迹规划。

         planner_plan_ = new std::vector();
            latest_plan_ = new std::vector();
            controller_plan_ = new std::vector();
            planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));