- 栅格角(goal_th)。以机器人中心坐标(src)和栅格坐标生成一条直线，该直线在world坐标系的角度。
- 栅格夹角(ang_diff)。机器人偏航角和栅格角之间角度，旋转方向是偏航角到栅格角。
- 计算窄区足迹只须考虑机器人宽度，不涉及长度。其长度固定0.15m。
a_star_search在局部代价地图成功找到一条路径(route.steps)后，便要进入窄道判断。窄道判断是根据这条路径，生成一系列变量。在生成这些变量时，核心是以着判断窄道思路，即判断路径左、右两侧的障碍物情况。
图1中，机器人接下要向东南走，蓝色是a_star_search得到的路径route。在route，用数值是整数的map坐标表示路径点。示例中，机器人中心在(30, 30)，此条路径终点是(36, 19)。
窄道判断总逻辑：沿着route路径，从第一个栅格src开始，最多直到最后一个在距离44.2cm(max_dist)内的，依次判断这些栅格两侧的障碍情况。
具体到图1，是从src(30, 30)开始，依次(31, 29)、(32, 28)、(33, 27)、(34, 26)。因为判断5个路径点后，已得出个结论，此次只向前(only_forward=true)，所以后面虽然还有44.2cm内栅格，可不必判断。
一、判断范围
1.1 前方要判断多远
变量max_dist指示最大要判断多远，值是“sim_time_dist * 1.3”。
sim_time_dist：它是在计算候选路径时，机器人在给定模拟时长内，以最大候选速度运行，能跑出的最大距离。目前是0.34m。速度sim_max_vel_x、sim_max_vel_y的合速度vmag是最大速度，模拟时长是default_config_.sim_time，它们相乘便是sim_time_dist。
图2显示了sim_time_dist、at_least_dist、max_dist的位置关系。
- b_f_dist：在这段距离内要判断path_all_back_、path_only_backward、path_expect_forward等标记。
- at_least_dist：该范围内的栅格一定需要判断障碍情况，意味着一定会放入global_plan。值等于sim_time_dist。。
- max_dist：最远要考虑的范围。(at_last_dist, max_dist]内，有突发拐弯的要被扔掉。
1.2 两侧多近的障碍物，才认为会让形成窄道
要能在某个栅格形成窄道，意味要在它左、右两侧存在障碍栅格，把它和两侧栅格合称为窄区。这里引入个概念：窄区足迹(narrow_footprint)。窄区足迹是在机器人足迹基础上，经过左(上)、右(下)延伸出来的足迹。其延升出来的部分，就是窄道判断时两侧要考虑的距离。
图3中，红色矩形是窄区足迹。长度0.15m，宽度0.6m。它怎么发挥作用？——想象下把此个窄区足迹中心放到图1中的第二个路径点(31, 29)，然后旋转角度，让等于第一个点(30, 30)到它形成的栅格角(goal_th)。此时对照图3，处在蓝色角度内、红色框的栅格将被于判断当前是否正处在窄道。依次类推，其它四个路径点一样处理。这里说下第一个点src(30, 30)，它无法生成要放置的角度，计算其角度方法是以它为起点，第二个点为终点的角度，即放置窄区时，src点角度和第二个路径点是一样的。
计算窄区长度使用了个表示半径的变量x_radius，值固定是0.075m。因而不管机器人是什么足迹，窄区足迹长度总是0.15m。
计算窄区宽度使用了个表示半径的变量y_radius，值基于机器人宽度计算。假设机器人宽度0.3m，那认为有道路要能过它，最窄需要min_narrow_width/MNW = 0.4(0.3 + resolution * 2)宽度。这个等式对照来说，机器人要能在这条路上走，那这条路的宽度至少得是机器人宽度加两个栅格宽度。再由“MNW – robot_width / 2 + resolution”，得到y_radius= 0.3。这里减去“robot_width/2”，以窄区左侧为例，最差情况是机器人完全贴着右侧路沿，此时中心最远距离是MNW – robot_width / 2。在这种情况下，左侧再加一个“resolution”，那就是窄区宽度。
| 足迹宽度 | 窄区宽度 |
| 0.2cm | 0.25cm x 2 |
| 0.25cm | 0.275cm x 2 |
| 0.3cm | 0.3cm x 2 |
| 0.4cm | 0.35cm x 3 |
在图3，处在蓝色角度、红色框内的栅格，将被用于判断当前是否正处在窄道。
二、is_narrow_point
is_narrow_point用于判断指定栅格是否正处于窄区。true表示正处于，即两侧都有障碍物，false则不在窄区。(x, y)是栅格world坐标，yaw是栅格角goal_th。
bool DWAPlannerROS::is_narrow_point(const tpose2d& local_2_map, double x, double y, double yaw,
std::set<tpoint>* front_obs_cells, std::set<tpoint>* back_obs_cells, bool verbose)
{
double min_threshold = DEG2RAD(30);
double max_threshold = DEG2RAD(150);
std::vector<SDL_Point> obs_cells;
tpose2d pose2d(x, y, yaw);
get_footprint_obs_cells(*costmap_ros_->getCostmap(), getNarrowFootprint(), pose2d, 0.00, 0.00, -1 * local_2_map.yaw,
obs_cells);
tpressure_result narrow_pressure;
resolve_pressure(false, pose2d, obs_cells, min_threshold, max_threshold, narrow_pressure, front_obs_cells,
back_obs_cells, verbose);
return narrow_pressure.front_has_obs && narrow_pressure.back_has_obs;
}is_narrow_point分两步。第一步调用get_footprint_obs_cells，得到在该栅格上，窄区涉及到哪些LETHAL_OBSTACLE栅格，结果放在obs_cells。第二步调用resolve_pressure，检查obs_cells中的障碍栅格是否左、右分布，从而得到该栅格是否正处于窄区。
2.1 get_footprint_obs_cells
get_footprint_obs_cells作用是对某个足迹，进行尺寸填充，并放在robot_pose位姿，然后收集涉及到栅格中有哪些是障碍物栅格，即栅格值LETHAL_OBSTACLE，结果放在obs_cells。
bool get_footprint_obs_cells(costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::Point>& footprint,
const tpose2d& robot_pose, double padding_x, double padding_y, double yaw_increment,
std::vector<SDL_Point>& obs_cells)针对is_narrow_point，参数footprint便是窄区足迹，因为要检查的都在窄区足迹涉及到栅格，不必尺寸填充，因而填充参考(padding_x, pqdding_y)填0。
以路径点(31, 29)为例，至此，obs_cells存放着窄区在该点时，涉及到的障碍栅格。
2.2 resolve_pressure
resolve_pressure作用是判断obs_cells内栅格，它们在水平方向或垂直方向，是否有存在于min_threshold和max_threshold联合指定的限定区域。
void DWAPlannerROS::resolve_pressure(bool front_back, const tpose2d& robot_pose2d,
const std::vector<SDL_Point>& obs_cells, double min_threshold, double max_threshold,
tpressure_result& result, std::set<tpoint>* front_obs_cells, std::set<tpoint>* back_obs_cells, bool verbose)
图4显示水平、垂直时，min_threshold和max_threshold如何联合指定限定区域。是水平方向时，栅格夹角落在区域1，认为是前面，反之，落在区域2的则为是后面。垂直方向时，落在区域1的，认为是上面/左侧；反之，区域2的是下面/右侧。这里的上面、下面，是针对机器人正朝向东，如果机器人朝向西，那上、下要颠倒。为表示更明确，往往会用左、右。机器人向西时，人脸朝着机器人方向面向西，此时左手方向（向南）是上面，右手方向（向北）是下面。
返回值存储在参数result，其类型tpressure_result。内中两个bool成员变量：front_has_obs和back_has_obs，前者表示前方或上面有障碍，后者表示后方或下面有障碍。这两值可同时是true。
针对is_narrow_point，front_back=false，要判断垂直方向，图4中的右侧。min_threshold=30、max_threshold=150，看栅格是否有落在上、面两个120度的区域内。
回到图1，针对路径上各个点，以各个路径点为参数调用is_narrow_point，根据各个点出来的两侧障碍物情况，形成相关变量。
三、三种路况：笔直(straight_ward_=true)，可能须要调头(maybe_uturn_=true)，其它
有三种路况，笔直、可能须要调头和其它，窄道判断的主要功能是判断出此时机器人正处于哪种。要判断是哪种，除要符合数量的栅格处在窄区外，还有栅格夹角。
图5显示了如何用栅格夹角判断路况。
- 前面3(min_check_path_at)个路径点都落在红色实线角度范围内，only_backward = true。
- 前面3(min_check_path_at)个路径点虽然没都落在红色实线角度范围内，但落在蓝色实线，maybe_uturn_ = true。
- 和only_backward相反，如果路径点夹角都落在紫色范围，即非红色范围，only_foreward是true。
由于only_backward一定全在maybe_uturn_范围，所以only_backward是true时，maybe_uturn_一定是true。
通过这三个表示夹角的变量，加上窄区情况，就可判断路况了。
3.1 第一优先级路况：笔直
笔直分笔直向前和笔直向后。以下是以笔直向前条件。
- 夹角条件。only_forward = true。
- 窄区路径点数条件。窄区数至少2(min_is_narrow_judges_forward)个。
一旦窄区数不符，即使夹角符合了，only_forward会改为false。
以下是笔直向后条件。
- 夹角条件。only_backward = true。
- 窄区路径点数条件。窄区数至少3(min_is_narrow_judges_backward)个。
一旦窄区数不符，即使夹角符合了，only_backward会改为false。
一旦要笔直向后，表示路径类型的path_case_会置为path_only_backward。
不论哪种笔直，一旦符合后，变量straight_ward_置为true。同时，会把maybe_uturn_置为false。会保证straight_ward_、maybe_uturn_不可能同时是true。
3.2 第二优级路况：可能须要调头
指示此种路况的标记是maybe_uturn_=true。
从图5可看出，如果maybe_uturn_=true，only_backward可能也是true，那这时还会不会only_backward=true？——不会。在判断第一优先级笔直时，一旦不符合，会把only_backward置为false。
至此，已判断出是哪种路况，接下是根据路况计算此个时间片要发送到机器人的速度。