ROS : Navigation - move_base 功能包源码分析

move_base 的整体框架

move_base 是一个开源 2D 移动机器人导航包，实现机器人自主导航到给定目标点的功能。 move_base 提供配置、运行以及跟导航栈交互的接口，基本框架如下图所示，其核心包含了 3 大功能模块，并以插件的形式加载到 move_base 中：

• 全局路径规划 nav_core::BaseGlobalPlanner
• 局部路径规划 nav_core::BaseLocalPlanner
• 恢复机制 nav_core::RecoveryBehavior

用户可以自定义任意的全局路径规划器、局部路径规划器，只需要将其跟 nav_core::BaseGlobalPlanner 、 nav_core::BaseLocalPlanner 接口对接上即可。这两个接口的实现在 nav_core 包 。

move_base 的输入输出

move_base 的 输入是一个 goal (geometry_msgs/PoseStamped) ， 输出是一系列 command (geometry_msgs/Twist) 。详细一点来说，就是在已知环境地图和机器人在地图中位置的情况下，利用地图和传感器数据处理得到的 global costmap 和 local costmap ，调用全局路径规划器得到一条全局路径，调用局部路径规划器使机器人在移动过程中能在大致沿着路径前进并动态规避障碍物，整个行进过程中还要处理一部分异常情况即 recovery_behaviors 。

move_base 通过订阅 /move_base_simple/goal 或者 /move_base/goal 话题消息接收目标点。发布一个目标点的两种方式示例如下：

root@eln-pc:~/catkin_ws# rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped "
header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: "map"
pose:
  position:
    x: 2.0
    y: -1.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.471599513115
    w: 0.881812848188
"

root@eln-pc:~/catkin_ws# rostopic pub /move_base/goal move_base_msgs/MoveBaseActionGoal "
header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  target_pose:
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs: 0
      frame_id: "map"
    pose:
      position:
        x: 2.0
        y: -1.0
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.474801462745
        w: 0.880092933147
"

move_base 中各函数的主要作用

• MoveBase ：构造函数
• goalCB ：将 goal 转换为 move_base_msgs::MoveBaseActionGoal 类型
• clearCostmapWindows ：清空机器人周围的正方形空间
• clearCostmapsService ：将 costmap 重置为 unknow
• planService ：服务 make_plan 的回调函数。接受 Service 的消息后，开始路径规划，得到 global_plan 的路径，跟 as_ 得到的 action 一样，都会 make_plan ，但是不做局部路径规划（只是提供了个全局路径规划的接口）
• ~MoveBase ：析构函数
• makePlan ：获取机器人的位姿作为起点，然后调用全局规划器的 makePlan 返回规划路径，存储在 planner_plan_
• publishZeroVelocity ：发布速度为 0 的 cmd_vel
• isQuaternionValid ：验证是否是四元数
• goalToGlobalFrame ：将 goal 转化为 geometry_msgs::PoseStamped 格式的 goal_pose_msg
• wakePlanner ：通过 boost thread 的条件变量 planner_cond_ 唤醒线程 planThread
• planThread ：是全局路径规划的线程，调用全局路径规划获取路径，同时保证规划的周期性以及规划超时清除 goal
• executeCb ：是 move_base 接收到新目标时调用的函数，执行局部路径规划和导航
• distance ：计算 p1 和 p2 两个点之间的距离
• executeCycle ：是局部路径规划的核心函数
• loadRecoveryBehaviors ：加载 Recovery 的行为
• loadDefaultRecoveryBehaviors ：加载默认的 Behaviors
• resetState