移动机器人导航navigation中的move_base功能包深度剖析(1)

针对移动机器人导航navigation中的move_base部分，详细剖析其实现过程。

旁边专栏还在更新ROS-Navigation 包其他源码分析，便于继续深入了解

---

---

前言

move_base 是一个开源 2D 移动机器人导航包，用于将机器人在指定的导航框架内运动到任务位置。 move_base 包执行一个完成给定导航任务的ROS行为，基于全局地图的路径规划是在机器人向下一个目的地出发前开始的，这个过程会考虑到已知的障碍物和被标记成“未知"的区域。要使机器人实际执行动作行为，本地路径规划器会监听传回来的传感器数据，并选择合适的线速度和角速度来让机器人完整地执行完全局路径规划上的当前段路径。附move_base功能包下载地址：点这里。

---

提示：以下是本篇文章正文内容

1. 相关源码文件

1.1 主要头文件（move_base/include）

• move_base.h

1.2 主要源代码文件（move_base/src）

• move_base_node.cpp
• move_base.cpp

2. 源码剖析

2.1 move_base包给定导航目标

使用MoveBaseActionGoal消息类型来指定目标，目标由一个包含一个frame_id的ROS标准header、一个goal_id和一个PoseStamped消息类型的goal组成。其中，PoseStamped消息类型是由一个header和一个包含position和orintation的pose组成。

2.2 move_base类内部结构

（1）创建3个插件

      pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;	// 全局路径规划器
      pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;	// 本地路径规划器
      pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;	// 恢复行为执行器

（2）创建3个规划区

      std::vector<geometry_msgs::PoseStamped>* planner_plan_;	// 规划位姿集
      std::vector<geometry_msgs::PoseStamped>* latest_plan_;	// 最新位姿集
      std::vector<geometry_msgs::PoseStamped>* controller_plan_;	// 控制位姿集

（3）创建规划线程

      bool runPlanner_;		// 执行规划标志位
      boost::recursive_mutex planner_mutex_;
      boost::condition_variable_any planner_cond_;
      geometry_msgs::PoseStamped planner_goal_;
      boost::thread* planner_thread_;

（4）构造函数MoveBase()

1. 创建多个节点句柄nh、private_nh、action_nh和simple_nh。

    ros::NodeHandle private_nh("~");
    ros::NodeHandle nh;
    ros::NodeHandle action_nh("move_base");
    ros::NodeHandle simple_nh("move_base_simple");

private_nh通过方法param(param_name, param_val, default_val)从参数服务器检索指示的值param_name,若成功，则将结果存储在param_val中，否则将default_val赋值给param_val；以及初始化节点，创建多个Server实例。
nh创建cmd_vel实例，向移动底盘应答速度指令。

    vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

action_nh创建goal、recovery_status实例

    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);

2. 初始化全局路径规划。

   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);
    }

3. 为控制器的代价地图创建 ros 捕捉器，并初始化一个我们将与底层地图一起使用的指针。

   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);
    }

4. 初始化本地路径规划。

   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);
    }

5. 基于传感器数据，开始激活代价地图信息

    planner_costmap_ros_->start();
    controller_costmap_ros_->start();

6. 陆续初始化move_base其余部分的组件

       //用于获取规划的实例
    make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);

    //用于清理代价地图的实例
    clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

    //关闭代价地图
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
      planner_costmap_ros_->stop();
      controller_costmap_ros_->stop();
    }

    //加载特定的恢复行为
    if(!loadRecoveryBehaviors(private_nh)){
      loadDefaultRecoveryBehaviors();
    }

    //用于执行规划时的初始化
    state_ = PLANNING;

    //执行恢复行为时的索引
    recovery_index_ = 0;

    //激活动作服务器
    as_->start();

    dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
    dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);

---

参考：
https://github.com/ros-planning/navigation/tree/kinetic-devel/move_base
move_base部分内容就是如上所述，内容较多，可以收藏一下，方便以后查看噢～～～