(四)configureBackupModes 1 处理 goal_idx 和 transformed_plan
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
void TebLocalPlannerROS::configureBackupModes(
std::vector<geometry_msgs::PoseStamped>& transformed_plan,
int& goal_idx)
{
ros::Time current_time = ros::Time::now();
// reduced horizon backup mode 上篇博客所列的第四种情况下

// we do not reduce if the goal is already selected , because the orientation
// might change -> can introduce oscillations
// no_infeasible_plans_ 改成 no_feasible_plans_ 比较合适
if (cfg_.recovery.shrink_horizon_backup &&
goal_idx < (int)transformed_plan.size()-1 &&
(no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() <
// keep short horizon for at least a few seconds
cfg_.recovery.shrink_horizon_min_duration ) )
{
ROS_INFO_COND(no_infeasible_plans_==1,
"Activating reduced horizon backup mode for at least %.2f sec (infeasible
trajectory detected).",
cfg_.recovery.shrink_horizon_min_duration);

// Shorten horizon if requested reduce to 50 percent:
int horizon_reduction = goal_idx/2;

if (no_infeasible_plans_ > 9)
{
ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected
10 times in a row: further reducing horizon...");
horizon_reduction /= 2;
}
// we have a small overhead (过头) here, since we already transformed 50%
// more of the trajectory. But that's ok for now, since we do not
// need to make transformGlobalPlan more complex
// reduced horizon 应当很少发生
int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1;
goal_idx -= horizon_reduction;

// 从 transformed_plan 的 new_goal_idx 开始,一直删除到尾部
if (new_goal_idx_transformed_plan >0 && goal_idx >= 0)
transformed_plan.erase(transformed_plan.begin() + new_goal_idx_transformed_plan, transformed_plan.end());
else
goal_idx += horizon_reduction; // this should not happen, but safety first
}
// ......第二部分......
}
  • shrink_horizon_backup: true. 在自动检测到问题(plan not feasible)的情况下,允许planner临时缩小范围(50%). TEB将以更近的点作为规划目标,尝试重新规划出可行路径。调试时可关闭,以在可视化界面上观察原出错路径。另请参阅参数shrink_horizo​​n_min_duration

  • shrink_horizon_min_duration: 10,单位秒。 如果检测到不可行的轨迹,激活缩小的水平线后备模式,本参数为其最短持续时间。个人怀疑这个参数无意义,因为它更新值的地方也都有no_infeasible_plans_变量的更新,判断时,只根据no_infeasible_plans_即可,详见代码configureBackupModes