现实环境中，关于Teb Local Planner 参数调试总结

1.问题一：针对阿克曼底盘控制的小车，方向控制“摆头”严重！
在Teb中，主要由几个参数决定：

根据de_ref参考两个位姿之间的时间差，dt_hysteresis为滞后时间，也就是为时间差加一个误差限，一般为dt_ref的10%。
经过调整滞后想要达到的目的是，每个时间差都在dt_ref左右。

 # ********************** Carlike robot parameters ********************
 min_turning_radius: 0.5        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
 wheelbase: 0.4                 # Wheelbase of our robot
 cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)
 # ********************************************************************

在车体模型参数中，轴距和转弯半径都会对其造成影响，cmd_angle_instead_rotvel会将控制命令转换成线速度和角度的方式，当轴距设置较大时，再经过反正切函数计算角度时，会将其角度的波段范围缩小，会有效的降低“摆头”情况，但是治标不治本。
通过设置de_ref也可以有效的降低“摆头”情况。

2.问题二：局部路径目标点设置错误，导致并不会严格按照全局路径寻迹，直接贯穿地图。
原因：在Teb中，切割全局路径时，将CostMap的一半作为阈值，如果当前位置在局部地图边界之外，则把全局地图的最后一个点设置为局部规划的目标点。
在Teb的transformGlobalPlan函数中：

    //we'll discard points on the plan that are outside the local costmap
    double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                     costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);

以上确定了距离的阈值，接下来计算全局路径中符合阈值的路径点，这里全局地图和局部地图都是在Map坐标系下，其实并没有转换。如果满足阈值要求，则加入到局部路径transformed_plan中去。

 //now we'll transform until points are outside of our distance threshold
    while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length))
    {
      const geometry_msgs::PoseStamped& pose = global_plan[i];
      tf::poseStampedMsgToTF(pose, tf_pose);
      tf_pose.setData(plan_to_global_transform * tf_pose);
      tf_pose.stamp_ = plan_to_global_transform.stamp_;
      tf_pose.frame_id_ = global_frame;
      tf::poseStampedTFToMsg(tf_pose, newer_pose);
      transformed_plan.push_back(newer_pose);

      double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
      double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;
      
      // caclulate distance to previous pose
      if (i>0 && max_plan_length>0)
        plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);
      ++i;
    }

如果没有满足要求的全局路径点，就会发生将全局路径最后一个点设置为局部路径的当前目标点，此时便会发生横穿地图的情况：

    if (transformed_plan.empty())
    {
      tf::poseStampedMsgToTF(global_plan.back(), tf_pose);
      tf_pose.setData(plan_to_global_transform * tf_pose);
      tf_pose.stamp_ = plan_to_global_transform.stamp_;
      tf_pose.frame_id_ = global_frame;
      tf::poseStampedTFToMsg(tf_pose, newer_pose);

      transformed_plan.push_back(newer_pose);
      
      // Return the index of the current goal point (inside the distance threshold)
      if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1;
    }

否则，将从全局路径中找到的符合要求的最后一个点，设置为局部路径的当前目标点current_goal_idx。

    else
    {
      // Return the index of the current goal point (inside the distance threshold)
      if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop
    }

因为在实际需求中，给定固定的全局路径，因此如果要避免这种情况，就要将局部地图CostMap的范围设大，此时会影响计算的效率，否则要时小车的当前位置靠近全局路径。如果参数设置不当，小车较容易偏离全局路径，如果此时不更新全局路径的话，就会直接奔向全局地图的最后一个点。
附上Teb代码解析博客 https://blog.csdn.net/windxf/article/details/110039280

在后续的调试中，发现是全局路径剪切问题，导致无法找到最近点，即参数global_plan_prune_distance: 0.6

double dist_thresh_sq = dist_behind_robot*dist_behind_robot;
    
    // iterate plan until a pose close the robot is found
    std::vector<geometry_msgs::PoseStamped>::iterator it = global_plan.begin();
    std::vector<geometry_msgs::PoseStamped>::iterator erase_end = it;
    while (it != global_plan.end())
    {
      double dx = robot.getOrigin().x() - it->pose.position.x;
      double dy = robot.getOrigin().y() - it->pose.position.y;
      double dist_sq = dx * dx + dy * dy;
      if (dist_sq < dist_thresh_sq)
      {
         erase_end = it;
         break;
      }
      ++it;
    }

通过参数擦除之前经过的全局路径点，默认全局路径中第一个点即最近点，因此并不会往后去寻找。
因为在一般规划中，全局路径会不断地更新，所以全局路径中第一个点即为最近点。
而在本次需求下，需要按照事先规定的全局路径，而此时巡逻开始的位置并不一定在全局路径的起点。

3.问题三：在Teb计算控制命令时，计算时间较长，发送频率过低导致控制不平滑，频繁刹车。
参考文章：https://www.it610.com/article/1295841594758471680.htm

TEB规划器的性能问题的总结参考
关闭多路径并行规划（效果非常显著）
使用Costmap Converter （非常显著）
降低迭代次数（no_inner/outer_iterations） （显著）
降低 max_lookahead_distance （一般）
减小局部耗费地图的大小 （显著）
增大规划周期和控制周期 （影响效果）
使用单点footprint，配合最小障碍物距离约束 （不太显著且影响效果）

Costmap Converter（ROS WIKI：costmap_converter）
TEB默认情况下不使用Costmap Converter。事实上，此插件可以在复杂场景下极大提高运算效率，尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低。

#当使用costmap_converter插件时
costmap_converter_plugin  #(字符串，默认值:" ")
#定义插件名称，以及将成本单元格转换成点/线/多边形。设置一个空字符串以禁用转换，以便将所有单元格都视为点障碍
costmap_converter_spin_thread #(bool,默认值:true)
#如果设置为true，costmap转换器将在另一个线程中调用其回调队列
costmap_converter_rate  #(double,默认值:5.0)
#用于定义插件处理当前成本图的频率

本地costmap_2d配置（强烈建议使用滚动窗口！）：
weight/height：局部成本图的大小：表示最大轨迹长度以及要考虑的占用单元数（对计算时间有重大影响，但是如果太小：较短的预测/规划范围会降低自由度，例如，为了避开障碍物）
resolution：解析局部成本图：精细的解析度（较小的值）意味着存在许多需要优化的障碍（对计算时间的重大影响）

在teb_local_planner/Tutorials/Frequently Asked Questions中有更多关于如何设置优化参数来降低计算时间。
Wiki链接：http://wiki.ros.org/teb_local_planner/Tutorials/Frequently%20Asked%20Questions

*关于Teb的博客：
ROS - teb_local_planner 参数总结
帮助路径规划理解

4.问题四：路径规划的过程中，小车原地转圈，但是全局路径正常
检查local cost map参数，height与width是否太小！
如果参数设置过小的话，局部路径无法生成就会原地打转！