navigation总览

本文为对base_local_planner源码解读的前置研究。

---

---

1. navigation总览

打开一个终端，在github目录下输入
　get clone https://github.com/ros-planning/navigation.git
　下载navigation包，将会得到许多文件夹。其中主要的包作用如下：
　
　nav_core

该包定义了整个导航系统关键包的接口函数，包括base_global_planner, base_local_planner以及recovery_behavior的接口。里面的函数全是虚函数，所以该包只是起到规范接口的作用，真正功能的实现在相应的包当中。之后添加自己编写的全局或局部导航算法也需要在这里声明。添加插件的总体描述以及教程链接

global_planner和navfn

这两个包干的事情是一样的，都是为实现目标点与当前点之间的全局路径规划，内部都有Dijkstra算法和A*导航算法的实现，ROS系统默认采用的是navfn。global_planner是新版并且已经稳定，仍然使用navfn是为了兼容性考虑，可以在nav_core中做修改。

base_local_planner

完成局部窗口内的路径规划任务，机器人行动速度的具体生成在此包当中完成。目前有两种局部路径规划算法实现，一是航迹推算法（TrajectoryROS），一是动态窗口法(DWA)，该包内部的默认实现是航迹推算法，但是留出了DWA的定义接口，DWA的实现在dwa_local_planner中。

map_server

主要功能是读取pgm和yaml配套的地图文件，并将之转换到/map话题发送出来（比如rviz中显示的地图就是订阅了该话题）， 另外还提供地图保存等增值服务。

costmap_2d

以层的概念来组织图层，用户可以根据需要自己配置，默认的层有static_layer（通过订阅map_server的/map主题）来生成， obstacle_layer根据传感器获得的反馈来生成障碍物图层， inflation_layer则是将前两个图层的信息综合进行缓冲区扩展。
　该包可进行动态窗口的生成（比如base_local_planner中使用的就是动态的局部窗口地图）也可以生成静态地图（比如global_planner中使用的就是全局静态地图）。（有待验证的疑问，是否静态地图中obstacle_layer不起作用，而动态地图中static_layer不起作用）。
　此外，该包还在实时发送自己的地图信息，通过nav_msgs::OccupancyGrid发送消息给rviz。所以可以仿照该写法来发送自己想要的数据到rviz中去。

rotate_recovery和clear_costmap_recovery

这两个包都继承自nav_core中定义的recovery_behavior类， 具体实现的是当导航发现无路可走的时候，机器人在原地打转转并清理（更新更恰当些）周围障碍物信息看是否有动态障碍物运动开，能找到路继续走。
　在move_base当中，默认使用的recovery_behavior是这样的，先进行一次旋转，然后进行一次小半径的障碍物更新，然后再进行一次旋转，再进行一次大范围的障碍物更新，每进行一次recovery_behavior，都会重新尝试进行一次局部寻路，如果没找到，才会再执行下一个recovery_behavior。
　这两个包的代码非常简单，不看也不会影响理解偏差，所以可以不用浪费时间看，当然因为简单要看也是分分钟的事。

move_base

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

接下来会从move_base开始着手导航的整个过程。

---

2. move_base

2.1 move_base概况

ROS提供的move_base包让我们能够在已建立好的地图中指定目标位置和方向后，move_base根据机器人的传感器信息控制机器人到达我们想要的目标位置。它主要功能包括：结合机器人码盘推算出的odometry信息，作出路径规划，输出前进速度和转向速度。这两个速度是根据在配置文件里设定的最大速度和最小速度而自动作出的加减速决策。下面的白色底色方框内就是move_base的内容：

必要的输入：

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

可选的输入：

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

输出：
　cmd_vel：在cmd_vel这个主题上发布Twist消息，这个消息包含的就是机器人的期望前进速度和转向速度。
　move_base收到goal以后，将目标goal通过基于actionlib的client（客户端）向服务器发送，服务器根据tf关系以及发布的odom消息不断反馈机器人的状态（feedbackcall）到客户端, 让move_base做路径规划和控制twist。
　知道了move_base的这些外围消息接口以后，move_base的运行还需要一些内部的配置参数，如机器人的最大最小速度，已经路径规划时的最大转弯半径等等，

2.2 move_base.launch分析

首先看的是move_base.launch中包括解析map，move_base和amcl定位三个部分，这构成了一个完整的框架 ：

map以及amcl部分不是我们关心的重点，所以打开tb_move_base_test.launch，其配置如下：

如上所示，使用的是global_planner这个包，它默认使用的是dijkstra，当然也可以使用A*全局路径规划，局部路径规划使用的是teb，同样需要配置上面第3行到第7行的一些yaml，这些yaml是costmap和planner的一些配置文件。关于这些这些文件设置的作用，请看这里。可以使用rosrun rqt_reconfigure命令实时对以上数据进行动态更改。
　总体的设置这样就完成了，之后是对navigation包中的全局路径规划和局部路径规划的源代码进行分析。

---

3.全局路径规划（暂时不涉及，留空）

---

4. 局部路径规划

既然想要知道局部路径规划开始的过程，自然要从nav_core中开始，点开其中的base_loacl_planner.h文件，我们关心的信息如下：

//最为关键的地方，计算机器人下一刻的速度
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
//判断是否到达目标点 
virtual bool isGoalReached() = 0; 
//加载全局路径 
virtual bool setPlan(const std::vector& plan) = 0; 
//初始化 
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

上文中关键部分为computeVelocityCommands，这个函数在trajectory_planner_ros.cpp，其主要实现框架如下所示：

bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { 
    //检查初始化、检查是否已经到达目标点...略 
    transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan);
    //如果已经到达目标点，姿态还没到 
    if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) { 
        tc_->updatePlan(transformed_plan); 
        //所以这个函数里最关键的子函数是findBestPath 
        Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds); 
        return true; 
        } 
    tc_->updatePlan(transformed_plan); 
    Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds); //然后又是转换，然后就发布出速度 }

下一步点开trajectory_planner.cpp，找到其中的子函数findBestPath,其主要实现框架如下：

Trajectory TrajectoryPlanner::findBestPath(tf::Stamped global_pose, tf::Stamped global_vel, tf::Stamped& drive_velocities) {
    
    //... 
    Trajectory best = createTrajectories(pos[0], pos[1], pos[2], vel[0], vel[1], vel[2], acc_lim_x_, acc_lim_y_, acc_lim_theta_); 
    //... 
    
}

最重要的函数是createTrajectories，是非常关键的函数，其源代码过长，直接给出主要实现框架：

Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta, double acc_x, double acc_y, double acc_theta) {
    //检查最终点是否是有效的，判断变量在updatePlan中被赋值
    if( final_goal_position_valid_ ) { 
        double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y ); 
        max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ ); 
        } 
    //是否使用dwa算法，sim_peroid_是1/controller_frequency_，暂时不清楚sim_period_和sim_time_的区别 
    if (dwa_) { 
    max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_); 
    min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_); 
    
    max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_); 
    min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_); 
    } 
    else { 
    max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
    min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_); 
    
    max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_); 
    min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_); } 
    //忽略其中的逻辑，按照不同的规则生成路径，调用的子函数是generateTrajectory

综上所述，整个base_loacl_planner的逻辑是这样的：
　computeVelocityCommands->findBestTrajectory –> createTrajectories –> generateTrajectory
　最终，选择分数最低的轨迹，发布出去。这便是整个局部规划器的实现思路和逻辑。