ros navigation-kinetic-stack 代码阅读

参考文章

navigation总览
Navigation源码阅读之dwa_local_planner（DWA动态窗口法）
关于move_base的原理及应用
ROS navigation分析：navigation框架

0. nav_core

该包定义了整个导航系统关键包的接口函数，包括base_global_planner, base_local_planner以及recovery_behavior的接口。
里面的函数全是虚函数，所以该包只是起到规范接口的作用，真正功能的实现在相应的包当中。

1. move_base

move_base可以说是整个navigation代码阅读的入口,整个导航系统的初始化也是在这里进行,其主要功能如下

这个是整个navigation stack当中进行宏观调控的看得见的手。
它主要干的事情是这样的：

• 维护一张全局地图（基本上是不会更新的，一般是静态costmap类型），维护一张局部地图（实时更新，costmap类型）;
• 维护一个全局路径规划器global_planner完成全局路径规划的任务， 维护一个局部路径规划器base_local_planner完成局部路径规划的任务。
• 然后提供一个对外的服务，负责监听nav_msgs::goal类型的消息，然后调动全局规划器规划全局路径，再将全局路径送进局部规划器，
• 局部规划器结合周围障碍信息（从其维护的costmap中查询），全局路径信息，目标点信息采样速度并评分获得最高得分的轨迹（即是采样的最佳速度），
• 然后返回速度值，由move_base发送Twist类型的cmd_vel消息上，从而控制机器人移动。完成导航任务。

move_base包运行后,我们仅需要在已经建立好的地图上指定目标位置和方向, move_base便会根据传感器(激光雷达,毫米波雷达等)获得的新的环境信息,以及自身的位置姿态,规划出一条全局的路径,然后局部路劲规划器根据机器人本身的状态和运动特性, 速度,加速度,位姿等规划出一系列的速度控制指令(并且进行速度模拟,碰撞检查).

白色底色方框内就是move_base的内容

必要的输入：

• goal ： 期望机器人在地图中的目标位置
• tf ： 各个座标系之间的转换关系。（具体/map frame --> /odom frame ，/odom frame --> /base_link frame）
• odom：根据机器人左右轮速度推算出的航向信息（即/odom座标系中机器人x,y座标以及航向角yaw，下面会具体介绍
• LaserScan：激光传感器的信息，用于定位。（在这个系列教程中，我们没有用到这个激光信息，而是在一个假的空白地图上对机器人进行控制，并假定/map座标系和/odom座标系完全重合，在后面会有关于这两个座标系的介绍）

可选的输入：

• amcl：自适应蒙特卡罗定位，一种用于2D环境下移动机器人的概率统计定位方法
• map：地图信息

输出：

• cmd_vel：在cmd_vel这个主题上发布Twist消息，这个消息包含的就是机器人的期望前进速度和转向速度。
  move_base收到goal以后，将目标goal通过基于actionlib的client（客户端）向服务器发送，服务器根据tf关系以及发布的odom消息不断反馈机器人的状态（feedbackcall）到客户端, 让move_base做路径规划和控制twist。

1.1 MoveBase::MoveBase(tf::TransformListener& tf)

主要完成系统的默认参数的设置, 系统默认规划器的选择, 如全局规划器与局部规划器的选择(下面会介绍).当然,这是代码中的默认参数,也可以通过launch载入配置文件.xml进行修改.

<!-- The move_base node -->
  <include file="$(find rbx1_nav)/launch/move_base.launch" /> 

MoveBase::MoveBase(tf::TransformListener& 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) {

    //get some parameters that will be global to the move base node
    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);  // disabled by default

    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

1.2 MoveBase::executeCb(move_base_goal)

executeCb(move_base_goal)函数结构如下:

1. executeCb(move_base_goal)函数收到运动的目标后,会首先转换到全局座标系下;
   然后开启MoveBase::planThread线程, 用于全局路径规划;
2. 开启costmap更新全局和局部costmap地图;
3. 执行executeCycle() , 局部路径规划与控制部分

2. 全局路径规划

全局路径规划navigation中一共有三个实现,分别是global_planner, navfn,carrot_planner,都是实现当前点与目标点之间的路径规划, (在上面的MoveBase中可以看到默认使用的是navfn),其中global_planner, navfn内部都有Dijkstra算法和A*导航算法的实现.

planThread 中planner->makePlan(start, goal, plan)的全局路径规划代码逻辑见下图;

3. 局部路径规划

同样局部路劲规划也有两种实现:一是航迹推算法（TrajectoryROS），一是动态窗口法(DWA),都使用了nav_core::BaseLocalPlanner父类提供的接口

3.1 executeCycle()中实现了以下功能:

• getRobotPose　//获取机器人当前位置
• publishFeedback　as＿发布位置状态反馈
• 是否走的足够远重置oscillation (避免震荡)
• if(new_global_plan_) 使用最新的全局路径
• move_base 状态机: move_base 在三种状态机中切换工作,PLANNING,CONTROLLING, CLEARING
  

3.2 tc_->computeVelocityCommands(cmd_vel) 计算局部控制速度并发布

• 1.将全局规划从规划帧转换到评价地图帧; transformGlobalPlan从全局路径中截取一定距离阈值内的路径作为transformed_plan
• 2.prunePlan 将之前的global路径点清除。（包括全局的global path和局部的global path）
• 3.robot_vel从发布的odom中获取速度信息
• 4.从全局plan中获取目标点座标
• ５．与目标点距离是否小于阈值，或者已进入阈值内
• 6.findBestPath(global_pose, robot_vel, drive_cmds);
• 7.传递控制指令cmd_vel
• 8.发布局部和全局plan

4. costmap_2d

ROS基础教程–CostMap_2D包的一些理解这篇博客写的十分详细了,可以参考这篇博客.
costmap_2d主要是一个2D地图的创建和管理, costmap_2d::Layer下有主要的四个子类,表示地图中的四个子层,costmap初始化之后,便开始以update_frequency的频率更新地图,从外界传感器获得数据(如激光雷达,pointcloud),然后更新在地图的相应层上.