一、ObstacleCostFunction
涉及到的配置变量
| max_trans_vel_ | __default__.max_vel_trans = 0.55 |
| max_scaling_factor_ | __default__.max_scaling_factor = 0.2 |
| scaling_speed_ | __default__.scaling_speed = 0.25 |
| sum_scores_ | false。不是配置变量，至少目前固定是false |
计算得分思路：依次把机器人中心放在轨迹中所有坐标点，算出机器人足迹涉及到栅格中最大的cost。>=0的返回值中，最大值是379，计算方法是INSCRIBED_INFLATED_OBSTACLE(253)*3/2=379。
double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { double cost = 0; double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_); double px, py, pth; if (footprint_spec_.size() == 0) { // Bug, should never happen ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?"); return -9; }
在计算轨迹各个点的footprintCost之前，会先计算缩放因子scale。如果当前平移速度小于scaling_speed_，则缩放因子为1.0，否则，缩放因子为(vmag - scaling_speed) / (max_trans_vel - scaling_speed) * max_scaling_factor + 1.0。然后，该缩放因子会被用于计算轨迹中各个点的footprintCost。
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) { traj.getPoint(i, px, py, pth); double f_cost = footprintCost(px, py, pth, scale, footprint_spec_, costmap_, world_model_);
对footprintCost，其传入参数中scale并没有使用，即目前还不能根据速度改变打分效果。footprintCost注释参考“CostmapModel::footprintCost”。它是把机器人中心放到特定坐标(x, y)，并且转特定角度(theat)后，机器人这个凸边形、N条边涉及到栅格中最大的cost值。为提高效率，它不计算足迹所有栅格的代价，认为只要计算N条边涉及到的栅格，就可计算出最大代价。这包括不计算中心点(x, y)栅格的代价。返回值中，最大值是379，计算方法是INSCRIBED_INFLATED_OBSTACLE(253)*3/2=379，因为比253大的LETHAL_OBSTACLE(254)、NO_INFORMATION(255)会分别返回-1、-2，表示不可行。“3/2”则是要单独提高INSCRIBED_INFLATED_OBSTACLE栅格的代价，对非INSCRIBED_INFLATED_OBSTACLE栅格，不会乘以”3/2“。
if(f_cost < 0){ return f_cost; } if(sum_scores_) cost += f_cost; else cost = std::max(cost, f_cost);
如果某个点的footprintCost为负，直接返回该负的得分。如果sum_scores_参数为false（目前固定是false），只取最大的得分，否则则累加所有得分。
} return cost; }
二、OscillationCostFunction
(摆动打分)尽量降低机器人在原地摆动的情况。所谓摆动是机器人往一个方向还没走出多远，就立刻反方向，摆动打分要让这个“立刻反方向”不通过。举个例子，沿x轴前进了一段距离，如果这距离还没到0.05米，就想后退，那这个希望的后退就认为是摆动，摆动打分用于避免出现“这个后退”。
- forward_pos_：在x轴，上一次是前进(xv_ > 0)。
- forward_neg_：在x轴，上一次是后退(xv_ < 0)。。
- forward_pos_only_。只有true时有效。在x轴，下一次只允许前进。
- forward_neg_only_。只有true时有效。在x轴，下一次只允许后退。
图2中，因为forward_neg_only_设置是true，即要求此次x轴必须前进，而候选速度中traj.xv_是大于0的0.003947，OscillationCostFunction认为这速度会导致摆动，无法通过摆动评测。
上面摆动平测失败是只要traj.xv_>0，很显然traj.xv_经常>0，可见forward_neg_only_这值应该是不断变化的。那它在哪变化呢？——updateOscillationFlags，更具体是内中的setOscillationFlags。
2.1 updateOscillationFlags
在DWAPlanner::findBestPath，一旦要用选出的最优轨迹result_traj_移动机人了，那就要以result_traj_为参数调用updateOscillationFlags，第二个参数值来自planner_util_->getCurrentLimits().min_vel_trans。
void OscillationCostFunction::updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory* traj,
double min_vel_trans) {
if (traj->cost_ >= 0) {此次局部规划出来的结果可能是没有可用轨迹，即result_traj_.cost_<0。
if (setOscillationFlags(traj, min_vel_trans)) {
prev_stationary_pos_ = pos;最近两次移动时，x、y、theta至少一个轴出现了反方向。
}
//if we've got restrictions... check if we can reset any oscillation flags
if(forward_pos_only_ || forward_neg_only_
|| strafe_pos_only_ || strafe_neg_only_
|| rot_pos_only_ || rot_neg_only_){
resetOscillationFlagsIfPossible(pos, prev_stationary_pos_);当前速度和上一次有出现反方向时的速度变换非常大时，那对下一次评测摆动不做任务速度方向限制，即肯定会通过。
} } }
2.2 setOscillationFlags
bool OscillationCostFunction::setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans) {
bool flag_set = false;
//set oscillation flags for moving forward and backward
if (t->xv_ < 0.0) {
if (forward_pos_) {
forward_neg_only_ = true;
flag_set = true;此次机器人会x-后退，而且上一次是x-前进，那要求下次机器人必须x-后退。即要避免出现“前进-后退-前进”。
} forward_pos_ = false; forward_neg_ = true;
设置此次的前进、后退标记。到下一次setOscillationFlags时，这两个值就是上一次的x方向情况了。
}
if (t->xv_ > 0.0) {
if (forward_neg_) {
forward_pos_only_ = true;
flag_set = true;此次机器人会x-前进，而且上一次是x-后退，那要求下次机器人必须x-前进。即要避免出现“后退-前进-后退”。
}
forward_neg_ = false;
forward_pos_ = true;
}
//we'll only set flags for strafing and rotating when we're not moving forward at all
if (fabs(t->xv_) <= min_vel_trans) {min_vel_trans是最小x、y合线速度。当x速度不超过合线速度时，才会考虑y、theta是否要出现摆动。
//check negative strafe
if (t->yv_ < 0) {
if (strafing_pos_) {
strafe_neg_only_ = true;
flag_set = true;
}
strafing_pos_ = false;
strafing_neg_ = true;
}
//check positive strafe
if (t->yv_ > 0) {
if (strafing_neg_) {
strafe_pos_only_ = true;
flag_set = true;
}
strafing_neg_ = false;
strafing_pos_ = true;
}
//check negative rotation
if (t->thetav_ < 0) {
if (rotating_pos_) {
rot_neg_only_ = true;
flag_set = true;此次机器人会theta-顺时针，而且上一次是theta-逆时针，那要求下次机器人必须theta-顺时针。即要避免出现“逆时针-顺时针-逆时针”。
}
rotating_pos_ = false;
rotating_neg_ = true;
}
//check positive rotation
if (t->thetav_ > 0) {
if (rotating_neg_) {
rot_pos_only_ = true;
flag_set = true;此次机器人会theta-逆时针，而且上一次是theta-顺时针，那要求下次机器人必须theta-逆时针。即要避免出现“顺时针-逆时针-顺时针”。
} rotating_neg_ = false; rotating_pos_ = true; } } return flag_set;
flag_set=true，意味着对下次候选速度判断时至少出现了一个禁止项。换句话说，最近两次移动时，x、y、theta至少一个轴出现了反方向，下一次要避免那轴上再出现和这次相反的方向了。
}
2.2 resetOscillationFlagsIfPossible
void OscillationCostFunction::resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev) {
double x_diff = pos[0] - prev[0];
double y_diff = pos[1] - prev[1];
double sq_dist = x_diff * x_diff + y_diff * y_diff;
double th_diff = pos[2] - prev[2];
//if we've moved far enough... we can reset our flags
if (sq_dist > oscillation_reset_dist_ * oscillation_reset_dist_ ||
fabs(th_diff) > oscillation_reset_angle_) {如果到这一次移出了较大距离（超过设置的阈值），那对下一次评测摆动不做任务速度方向限制，即肯定会通过。
sq_dist都是距离变换量的平方。在图1，阈值oscillation_reset_dist_是0.05，也就是说，前后两次移出的距离超过5厘米，或线速度变化量超过0.2，那下一次就不做摆动评测。
resetOscillationFlags(); } }
resetOscillationFlags作用是把成员变量都置为false，像forward_pos_、forward_neg_、forward_pos_only_、forward_neg_only_。