(四)configureBackupModes 2 FailureDetector和振荡恢复|路径规划TEB算法

configureBackupModes 第二部分

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
// detect and resolve oscillations
if (cfg_.recovery.oscillation_recovery)
{
    double max_vel_theta;
    // 车一般不能倒退，所以基本是 max_vel_x
    double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_.robot.max_vel_x : cfg_.robot.max_vel_x_backwards;
    if (cfg_.robot.min_turning_radius!=0 && max_vel_current>0)
        max_vel_theta = std::max( max_vel_current/std::abs(cfg_.robot.min_turning_radius),  cfg_.robot.max_vel_theta );
    else
        max_vel_theta = cfg_.robot.max_vel_theta;

    failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, max_vel_theta,
    cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps);
        
    bool oscillating = failure_detector_.isOscillating();
    // 距离上次振荡过了多久，若小于配置值(10秒)，说明最近也振荡了
    bool recently_oscillated = (ros::Time::now()-time_last_oscillation_).toSec() < cfg_.recovery.oscillation_recovery_min_duration;
    // 如果现在是振荡状态
    if (oscillating)
    {
        if (!recently_oscillated)    //  最近的状态不是振荡
        {
            // save current turning direction
            // 以下其实都是对 last_preferred_rotdir_ 赋值
            if (robot_vel_.angular.z > 0)
                last_preferred_rotdir_ = RotType::left;
            else
                last_preferred_rotdir_ = RotType::right;
            ROS_WARN("TebLocalPlannerROS: possible oscillation (of the robot 
                or its local plan) detected. Activating recovery strategy 
                (prefer current turning direction during optimization) ");
        }
        time_last_oscillation_ = ros::Time::now();
        // 对 prefer_rotdir_ 赋值，最终影响 AddEdgesPreferRotDir
        planner_->setPreferredTurningDir(last_preferred_rotdir_);
    }
    // 现在和最近都没有振荡   clear recovery behavior
    else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) 
    {
        last_preferred_rotdir_ = RotType::none;
        planner_->setPreferredTurningDir(last_preferred_rotdir_);
        ROS_INFO("TebLocalPlannerROS: oscillation recovery disabled/expired.");
    }
}

但是如果机器人确实stucked，最终会在move_base里Abort

setPreferredTurningDir

设置期望的初始转弯方向 (也就是惩罚相反的方向)。A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two solutions (in the same equivalence class!) with similar cost. 配置参数以调整惩罚的权重，Initial 说明惩罚只适用于trajectory的前几个位姿

参数dir可以是RotType::left, RotType::right, RotType::none

代码实际调用的是：

1
2
3
4
5
6
7
8
void HomotopyClassPlanner::setPreferredTurningDir(RotType  dir)
{
  // set preferred turning dir for all TEBs
  for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
  {
    (*it_teb)->setPreferredTurningDir(dir);
  }
}

结果还是TebOptimalPlanner的同名成员函数，赋值而已

1
virtual void setPreferredTurningDir(RotType dir) {  prefer_rotdir_ = dir;  }

赋值完了，prefer_rotdir_会在AddEdgesPreferRotDir里产生影响

AddEdgesPreferRotDir

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
void TebOptimalPlanner::AddEdgesPreferRotDir()
{
    /* roesmann 注释: these edges can result in odd predictions, in particular
    we can observe a substantional mismatch between open- and closed-loop planning
   leading to a poor control performance.
   目前这个功能只用于振荡恢复，短期使用这个约束不会有重大影响
   可能让机器人脱离振荡 */
  if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir == 0)
        return;
    // 必须定义好转向
  if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left)
  {
    ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation.");
    return;
  }
  // create edge for satisfiying kinematic constraints
  Eigen::Matrix<double,1,1> information_rotdir;
  information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);
  // 目前只使用前三个旋转
  for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i)
  {
    EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir;
    rotdir_edge->setVertex(0, teb_.PoseVertex(i));
    rotdir_edge->setVertex(1, teb_.PoseVertex(i+1));      
    rotdir_edge->setInformation(information_rotdir);
    
    if (prefer_rotdir_ == RotType::left)
        rotdir_edge->preferLeft();
    else if (prefer_rotdir_ == RotType::right)
        rotdir_edge->preferRight();
    
    optimizer_->addEdge(rotdir_edge);
  }
}

调用时在configureBackupModes第二部分，那里是对prefer_rotdir_赋值，

FailureDetector

类FailureDetector的代码在TEB的recovery_behaviors.cpp，FailureDetector检测机器人是否 stucked或者振荡，分析last N(buffer的大小)个速度命令，看这些线速度和角速度的平均值是否小于oscillation_v_eps 和 oscillation_omega_eps

构造函数和析构全是空。

buffer

1
2
3
// Set buffer length (measurement history)
// length: number of measurements to be kept
void setBufferLength(int length) {  buffer_.set_capacity(length);  }

调用：

1
2
failure_detector_.setBufferLength(std::round(
    cfg_.oscillation_filter_duration * controller_frequency ) );

默认是10*30，其实比较大，可以降低oscillation_filter_duration

1
2
// Clear buffer， 置为不振荡状态
void clear();

FailureDetector::update

先看调用，只有第一个参数是速度控制量，其他都是配置参数:

1
2
3
4
failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, 
    max_vel_theta, cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps);

bool oscillating = failure_detector_.isOscillating();

源码其实也简单

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
void FailureDetector::update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)
{
    if (buffer_.capacity() == 0)
        return;
    VelMeasurement measurement;
    /* 
    struct VelMeasurement
    {
        double v = 0;
        double omega = 0;
    };  */
    measurement.v = twist.linear.x; // 不考虑 y 方向速度
    measurement.omega = twist.angular.z;
    
    if (measurement.v > 0 && v_max>0)
        measurement.v /= v_max;
    else if (measurement.v < 0 && v_backwards_max > 0)
        measurement.v /= v_backwards_max;
    
    if (omega_max > 0)
        measurement.omega /= omega_max;
    
    buffer_.push_back(measurement);
    // immediately compute new state
    detect(v_eps, omega_eps);
}

detect

  • v_eps: (0,1)内的normalized 线速度的平均值的阈值，默认0.1
  • omega_eps: (0,1)内的normalized 角速度的平均值，默认0.1

如果机器人stucked则返回true，否则false

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
bool detect(double v_eps, double omega_eps)
{
    oscillating_ = false;
    //至少有一半的速度数据才检测，也就是过一点时间才判断
    if (buffer_.size() < buffer_.capacity()/2)  
        return false;
    double n = (double)buffer_.size();
    // 计算buffer内线速度和角速度的均值
    double v_mean=0;
    double omega_mean=0;
    int omega_zero_crossings = 0;
    for (int i=0; i < n; ++i)
    {
        v_mean += buffer_[i].v;
        omega_mean += buffer_[i].omega;
        if ( i>0 &&   g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )
            ++omega_zero_crossings;
    }
    v_mean /= n;
    omega_mean /= n;
    if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps 
            && omega_zero_crossings>1 )
    {
        oscillating_ = true;
    }
    return oscillating_;
}

参数

我用的参数值如下：

1
2
3
4
5
6
7
shrink_horizon_backup = true;
shrink_horizon_min_duration = 10:   # 默认10
oscillation_recovery = true;
oscillation_v_eps = 0.1;     # 线速度和角速度设置0.1已经足够
oscillation_omega_eps = 0.1;
oscillation_recovery_min_duration = 5;
oscillation_filter_duration = 6   # 默认10，这个大了会使buffer太大

要出现振荡，最直观的就是车左右摇摆或者车前后小步行走，因为代码中没有绝对值，否则朝一个方向小幅行走也算振荡了。实际中当然是不允许这种奇怪的行为。

(十一) optimizeTEB 2 初始化g2o求解器和添加约束|路径规划TEB算法

g2o边和顶点的定义

所有边都在头文件base_teb_edges.h中，顶点就是g2o_types\vertex_timediff.h 和 g2o_types\vertex_pose.h。两个顶点都比较简单，值得注意的是vertex_timediff.h的setToOriginImpl

1
2
3
4
virtual void setToOriginImpl()
{
  _estimate = 0.1;  // 不是0，不明白为什么
}

一元边：

1
2
template <int D, typename E, typename VertexXi>
class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>

  • EdgeViaPoint：以指定的经过点位置与待优化位姿（顶点）的误差作为目标。（保证轨迹会经过指定点）

  • EdgeObstacle：以障碍物与待优化位姿（顶点）的距离经过惩罚函数后的输出作为目标（保证离障碍物大于一定距离）

  • EdgeTimeOptimal：以时间间隔dt最小作为目标（保证时间最短轨迹）

二元边
二元边

1
2
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>

主要是考虑了运动学的约束关系（车的相邻的两个位姿态必须落在一个圆弧上）。二元边的两个顶点分别是两个位姿，还限制了最小转弯半径。

多元边（构造函数中resize顶点的个数）：

1
2
template <int D, typename E>
class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>

  • EdgeVelocity：以实际速度和限定最大速度经过惩罚函数的输出作为目标函数，顶点为相邻的位姿和时间间隔。（限制实际速度不超过最大，程序中引入了sigmoid函数决定速度的符号（根据论文内的说法，引入此函数是因为优化算法只能求解连续函数）

  • Edgevelocityholonomic：与EdgeVelocity的区别在于，ds是机器人本体坐标系下的，然后去求速度在本体坐标系下的表示。其余思路一致


1
2
3
class VertexPose : public g2o::BaseVertex<3, PoseSE2 >

class VertexTimeDiff : public g2o::BaseVertex<1, double>

初始化 optimizer

planner_初始化在if (cfg_.hcp.enable_homotopy_class_planning)的判断里，然后实际调用TebOptimalPlanner::initialize或者HomotopyClassPlanner::initialize，前者调用了TebOptimalPlanner::initOptimizer

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
boost::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
{
    // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
    static boost::once_flag flag = BOOST_ONCE_INIT;
    // 将为TEB定义的顶点和边注册到g2o::Factory
    boost::call_once(&registerG2OTypes, flag);  

    // allocating the optimizer
    boost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
    //        linear solver utilized for optimization
    // 针对稀疏矩阵，写死为 g2o::LinearSolverCSparse
    // typedef  g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
    TEBLinearSolver* linearSolver = new TEBLinearSolver(); // see typedef in optimization.h
    linearSolver->setBlockOrdering(true);

    //      block  solver utilized for optimization
    // typedef   g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> >  TEBBlockSolver;
    TEBBlockSolver* blockSolver = new TEBBlockSolver(linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver = 
                new g2o::OptimizationAlgorithmLevenberg(blockSolver);

    optimizer->setAlgorithm(solver);
    optimizer->initMultiThreading(); // required for >Eigen 3.1

    return optimizer;
}

注意算法求解器写死为 利文伯格法 和 g2o::LinearSolverCSparse


构建超图 buildGraph

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
bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{
  // 构建图优化问题。若顶点和边非空，则 add
  if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
  {
    ROS_WARN("Cannot build graph, because it is not empty. Call graphClear() ");
    return false;
  }
  // 增加图的顶点，TimedElasticBand类型的路径点和dt为顶点
  // 添加顺序会影响 稀疏矩阵的结构，进而影响优化效率
  AddTEBVertices();
  // add Edges (local cost functions)
  // legacy_obstacle_association 
  /*已经修改了将轨迹姿势与优化障碍联系起来的策略（参见changelog）。 
    您可以通过将此参数设置为true来切换到 旧策略。 
     旧策略：对于每个障碍，找到最近的TEB姿势; 新策略：对于每个teb姿势，只找到“相关”的障碍  */
  if (cfg_->obstacles.legacy_obstacle_association)   // false
      AddEdgesObstaclesLegacy(weight_multiplier);
  else
      AddEdgesObstacles(weight_multiplier);   // 参数写死为 2

  if (cfg_->obstacles.include_dynamic_obstacles)
      AddEdgesDynamicObstacles();
   /* error为其连接的Pose顶点的位置到这个Viapoint的距离的模长。
     信息矩阵为1x1的矩阵，其值为 weight_viapoint
     添加全局路径约束, 取决于参数 global_plan_viapoint_sep */
  AddEdgesViaPoints();

  // 对于TimedElasticBand类型中的位姿顶点序列-1条边，设置该边的顶点和权重矩阵，添加该边
  AddEdgesVelocity();
  AddEdgesAcceleration();
  AddEdgesJerk();   // 实际不执行
  // error直接就是连接的VertexTimeDiff的dt本身,信息矩阵为1x1矩阵，其值为weight_optimaltime
  AddEdgesTimeOptimal();   //添加时间约束
  
  if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
    AddEdgesKinematicsDiffDrive(); // differential drive robot
  else
    AddEdgesKinematicsCarlike(); // carlike robot since the turning radius is bounded from below.

  AddEdgesPreferRotDir();   // 实际不执行
  return true;  
}

添加 vertex

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
void TebOptimalPlanner::AddTEBVertices()
{
  // add vertices to graph
  ROS_DEBUG_COND(cfg_->optim.optimization_verbose, "Adding TEB vertices ...");
  unsigned int id_counter = 0; // used for vertices ids
  // std::vector<ObstContainer> obstacles_per_vertex_; 对应n-1 initial vertices
  obstacles_per_vertex_.resize(teb_.sizePoses());
  auto iter_obstacle = obstacles_per_vertex_.begin();
  for (int i=0; i < teb_.sizePoses(); ++i)
  {
      // 两种 vertex 都要添加
      teb_.PoseVertex(i)->setId(id_counter++);
      optimizer_->addVertex(teb_.PoseVertex(i) );
      if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs() )
      {
          teb_.TimeDiffVertex(i)->setId(id_counter++);
          optimizer_->addVertex(teb_.TimeDiffVertex(i) );
      }
      iter_obstacle->clear();
      // 这里和 AddEdgesObstacles 有关
      (iter_obstacle++)->reserve( obstacles_->size()  );
  }
}

这里的Vertex没有调用setEstimate函数，不设置初值。

添加约束

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
void TebOptimalPlanner::AddEdgesVelocity()
{
  // 只看 non-holonomic robot
    if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_theta==0)
        return;

    int n = teb_.sizePoses();
    Eigen::Matrix<double,2,2> information;
    // 对角线元素
    information(0,0) = cfg_->optim.weight_max_vel_x;
    information(1,1) = cfg_->optim.weight_max_vel_theta;
    information(0,1) = 0.0;
    information(1,0) = 0.0;
    // 对所有的位姿，信息矩阵都是相同的
    for (int i=0; i < n - 1; ++i)
    {
      EdgeVelocity* velocity_edge = new EdgeVelocity;
      // 速度约束有三个顶点， 多元边
      velocity_edge->setVertex(0,teb_.PoseVertex(i));
      velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
      velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));

      velocity_edge->setInformation(information);
      velocity_edge->setTebConfig(*cfg_);
      optimizer_->addEdge(velocity_edge);
    }
}

error有两项，分别是线速度与线速度线速度是否在设定好的区间内。

只要看懂了论文和g2o的应用套路，这里的添加的edge大部分都能看懂。比较复杂而且重要的是AddEdgesObstacles 和 AddEdgesViaPoints。 via_point规定了轨迹应当经过这些点，否则会产生相应的cost。via_point边会与原规划的路径中与其距离最近的Pose顶点相连。

对于加速度约束，还要注意 vel_start_.first 和 vel_goal_.first，前者一般会添加，后者基本是false，信息矩阵一直是同一个。




1
2
3
4
5
6
7
8
9
10
11
void TebOptimalPlanner::clearGraph()
{
  // clear optimizer states
  if (optimizer_)
  {
    // neccessary, because optimizer->clear deletes pointer-targets 
    // therefore it deletes TEB states
    optimizer_->vertices().clear();  
    optimizer_->clear();
  }
}

局部规划可以让路径显示graph，在rviz中配置

参考：
TEB中的g2o问题的设置

(十四) 可行性 isTrajectoryFeasible|路径规划TEB算法
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
// Do not allow config changes during the following optimization step
boost::mutex::scoped_lock cfg_lock(cfg_.configMutex());
bool success = planner_->plan(robot_pose_, robot_goal_, &robot_vel_, 
               cfg_.goal_tolerance.free_goal_vel); // straight line init
if (!success)
{
  planner_->clearPlanner();  // force reinitialization for next time
  ROS_WARN("not able to obtain a local plan for the current setting");
  
  ++no_infeasible_plans_;  // increase number of infeasible solutions in a row
  time_last_infeasible_plan_ = ros::Time::now();
  last_cmd_ = cmd_vel;
  return false;
}
// Check feasibility (but within the first few states only)
if(cfg_.robot.is_footprint_dynamic)
{
  // Update footprint of the robot and minimum and maximum distance 
  // from the center of the robot to its footprint vertices.
  footprint_spec_ = costmap_ros_->getRobotFootprint();
  costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
  // is a circular footprint
  if( robot_circumscribed_radius / robot_inscribed_radius_ < 1.02 )   
      cfg_.obstacles.min_obstacle_dist = robot_circumscribed_radius; 
   // fit dynamic footprint
  cfg_.robot.robot_inscribed_radius =  robot_inscribed_radius_;
  cfg_.robot.robot_circumscribed_radius = robot_circumscribed_radius;
}

isTrajectoryFeasible

接下来算法遍历前n个位姿(n就是参数feasibility_check_no_poses)，从当前的机器人位姿开始，检查这几个位姿点是否有碰撞。为了检测是否发生碰撞，validation-model可能用于optimization的footprint更复杂

参数feasibility_check_no_poses不能太大，因为optimizer可能不完全收敛。小的障碍碰撞在未来可以获得纠正，所以设置太大导致的失败不一定是有效的。

狭窄环境中，调试要慎重。局部规划可能拒绝一个infeasible trajectory，但全局规划会认为全局路径是feasible，结果就是机器人stucked。 如果禁止了后退速度，会经常出现not feasible，尤其是在cluttered环境

源码中是这样调用的:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, 
      robot_inscribed_radius_,  robot_circumscribed_radius, 
      cfg_.trajectory.feasibility_check_no_poses);
if (!feasible)
{
    cmd_vel.linear.x = 0;   cmd_vel.linear.y = 0;   cmd_vel.angular.z = 0;
    // reset everything to start again with the initialization of new trajectories.
    planner_->clearPlanner();
    ROS_WARN("trajectory is not feasible. Resetting planner...");
    ++no_infeasible_plans_;
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel;
    return false;
}

如果用的是HomotopyClassPlanner，实际上还是对best TebOptimalPlanner验证轨迹可行性，即TebOptimalPlannerPtr best = findBestTeb();，实际用的是同名函数

  • costmapmodel: costmap model的指针, `costmap_model = boost::makeshared<
    base_local_planner::CostmapModel>(*costmap    );`

  • footprintspec: 机器人世界坐标系的轮廓, `costmap_ros->getRobotFootprint();`

  • look_ahead_idx: 其实就是参数feasibility_check_no_poses，沿着trajectory需要验证的位姿点，
    如果是-1，那就验证整个trajectory。

这里的障碍物是以代价地图代表，而不是算法内部的ObstacleContainer

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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
bool TebOptimalPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model,
              const std::vector<geometry_msgs::Point>& footprint_spec,
              double inscribed_radius,  double circumscribed_radius, 
              int look_ahead_idx )
{
  if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses() )
      look_ahead_idx = teb().sizePoses() - 1;
  
  for (int i=0; i <= look_ahead_idx; ++i)
  {
    // 只考虑覆盖一个障碍的情况，撞了则直接 return
    if ( costmap_model->footprintCost(teb().Pose(i).x(), 
        teb().Pose(i).y(), 
        teb().Pose(i).theta(), 
        footprint_spec, inscribed_radius, circumscribed_radius) == -1 )
    {
      if (visualization_)
          visualization_->publishInfeasibleRobotPose(teb().Pose(i), *robot_model_);
      
      return false;
    }
    // 检查两个位姿点的间距是否比机器人内接半径大；或者它们的偏航角差距大于
    // 参数min_resolution_collision_check_angular
    // 如果符合一种情况， 需要增加插值(临时位姿点)
    // 如果两个位姿点被障碍物push away，那么它们的中心可能与障碍碰撞
    if (i < look_ahead_idx )
    {
      double delta_rot = g2o::normalize_theta( g2o::normalize_theta(teb().Pose(i+1).theta()) -
                         g2o::normalize_theta(teb().Pose(i).theta() )  );
      Eigen::Vector2d delta_dist = teb().Pose(i+1).position()-teb().Pose(i).position();
      if( fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular || 
          delta_dist.norm() > inscribed_radius  )
      {
        int  n_additional_samples = std::max( std::ceil(fabs(delta_rot) / 
          cfg_->trajectory.min_resolution_collision_check_angular ), 
                std::ceil(delta_dist.norm() / inscribed_radius) ) - 1;
        ROS_INFO("n_additional_samples: %d", n_additional_samples);
        PoseSE2  intermediate_pose = teb().Pose(i);
        for(int step = 0; step < n_additional_samples; ++step)
        {
          intermediate_pose.position() = intermediate_pose.position() + delta_dist / 
             (n_additional_samples + 1.0);
          intermediate_pose.theta() = g2o::normalize_theta(
            intermediate_pose.theta() + delta_rot / (n_additional_samples + 1.0) );

          if ( costmap_model->footprintCost(intermediate_pose.x(), 
                intermediate_pose.y(), intermediate_pose.theta(),
                footprint_spec, inscribed_radius, circumscribed_radius) == -1 )
          {
            if (visualization_) 
               visualization_->publishInfeasibleRobotPose(intermediate_pose, *robot_model_);
            
            return false;
          }
        }
      }
    }
  }
  return true;
}

实质上比较简单，判断路径feasible用的是base_local_planner::CostmapModel的footprintCost函数，位姿点间距大则增加插值。 详细参考文章CostmapModel 判断是否碰撞

有时会无法把footprint point转为map坐标系的坐标，因为它在局部代价地图之外。如果isTrajectoryFeasible函数检查的局部路径点太多，有的会超出局部代价地图，这样会一直返回false

小车撞障碍但又是 isTrajectoryFeasible

日志:

1
2
3
4
5
footprint cost in isTrajectoryFeasible: -1.000000
[17:17:26.883 - WARN] ():   trajectory is not feasible. Resetting planner..
[17:17:26.940 - INFO] (): footprint cost in isTrajectoryFeasible: 0.00
[17:17:26.940 - INFO] (): footprint cost in isTrajectoryFeasible: 0.00
# 后面一直是 0

costmap_model->footprintCost如果是-1，就是撞了障碍，但只有第一次是-1，后面全是0. 撞一次就离开了障碍，自然是0. 之所以继续撞，是因为路径规划的开头，手动让机器人位姿先调整到transformed_plan的第一个元素的朝向，这二者没有关系。

(九) TimedElasticBand 类和优化之前的准备|路径规划TEB算法

本文是对上一篇的补充

TimedElasticBand在timed_elastic_band.h，此类定义了一条时变弹性带的轨迹模型，只有两个protected成员

1
2
3
4
5
6
7
8
9
10
//Internal container storing the sequence of optimzable pose vertices
PoseSequence pose_vec_;
// Internal container storing the sequence of optimzable timediff vertices
TimeDiffSequence timediff_vec_;


// Container of poses that represent the spatial(空间的) part of the trajectory
typedef std::vector<VertexPose*> PoseSequence;
// Container of time differences that define the temporal(时间的) of the trajectory
typedef std::vector<VertexTimeDiff*> TimeDiffSequence;

类VertexPose是g2o类型，在g2o_types\vertex_pose.h，保存和封装SE2 pose (position and orientation) into a vertex(图顶点)，用于g2o的图优化。 类VertexTimeDiff是g2o类型，在g2o_types\vertex_timediff.h，保存和封装时间差deltaT为一个vertex，用于g2o的图优化.

在TEB初始化的过程中，将起点的Pose与终点的Pose所在的顶点设为固定的，使得g2o不对这两个Pose进行优化。

初始化某一个TEB时，其路线简单的由起点和终点的连线组成。在这条直线上均匀采样点作为待优化的顶点。采样的步长由参数min_samples决定。而 timediff 顶点的初始值为 步长除以参数max_vel_x。

每有一个pose顶点就产生一个time_diff顶点，time_diff顶点实际上是每两个Pose之间所需要的时间。

初始化轨迹 initTrajectoryToGoal

以指定的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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
bool TimedElasticBand::initTrajectoryToGoal(
            const std::vector<geometry_msgs::PoseStamped>&  plan, 
            double  max_vel_x, double  max_vel_theta, bool  estimate_orient, 
            int  min_samples, bool  guess_backwards_motion )
{
  if (!isInit()) 	// timediff_vec_  pose_vec_ 都是空
  {
    PoseSE2 start(plan.front().pose);
    PoseSE2 goal(plan.back().pose);
     //   double dt = 0.1;
    addPose(start); // 向 pose_vec_ 添加 起点   此时的 BackPose()是 start
    //StartConf 称为固定约束，第一个参数是 index
    setPoseVertexFixed(0,true);  // pose_vec_.at(index)->setFixed(status);   

    bool backwards = false;
    // 参数 guess_backwards_motion 在这里
    // check if the goal is behind the start pose (w.r.t. start orientation)
    if ( guess_backwards_motion && 
    ( goal.position()-start.position()).dot(start.orientationUnitVec()) < 0)
        backwards = true;
    // TODO: dt ~ max_vel_x_backwards for backwards motions
      // 取自全局路径点
    for (int i=1; i<(int)plan.size()-1; ++i)
    {
        double yaw;
        if (estimate_orient)
        {
            // get yaw from the orientation of the distance vector
            // between pose_{i+1} and pose_{i}
            double dx = plan[i+1].pose.position.x - plan[i].pose.position.x;
            double dy = plan[i+1].pose.position.y - plan[i].pose.position.y;
            yaw = std::atan2(dy,dx);
            if (backwards)
                yaw = g2o::normalize_theta(yaw+M_PI);
        }
        else 
        {
            yaw = tf::getYaw(plan[i].pose.orientation);
        }
        PoseSE2 intermediate_pose(plan[i].pose.position.x, plan[i].pose.position.y, yaw);
        /* 根据最大线速度和角速度，估计从 BackPose 到 intermediate_pose所花的时间，
          假设匀速运动。也就是说这是估算了一个最小值  */
        double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
        // intermediate_pose添加到 pose_vec_, 约束false, 它成为BackPose()
        // dt 添加到 timediff_vec_ ,  约束false
        addPoseAndTimeDiff(intermediate_pose,  dt);
    }
    // number of samples 小于参数 min_samples, insert manually
    // -1 的意思是不包含goal point，它在之后添加
    if ( sizePoses() < min_samples-1 )
    {
      ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than 
        specified by min_samples. Forcing the insertion of more samples ...");
      while (sizePoses() < min_samples-1)
      {
        // 策略: interpolate between the current pose and the goal
        // 只处理了 BackPose() 到 goal 这一段，而且是逐步二分
        PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
        double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
        // let the optimier correct the timestep (TODO: better initialization
        addPoseAndTimeDiff( intermediate_pose, dt ); 
      }
    }
    // Now add final state with given orientation
    double dt = estimateDeltaT(BackPose(), goal, max_vel_x, max_vel_theta);
    // goal 也插入 pose_vec_, 成为 BackPose()
    addPoseAndTimeDiff(goal, dt);
    // GoalConf 也成为了固定约束
    setPoseVertexFixed(sizePoses()-1,true); 
  }
  else   // size!=0
  {
    ROS_WARN("Cannot init TEB between given configuration and goal, because 
      TEB vectors are not empty or TEB is already initialized (call this function 
      before adding states yourself)!");
    ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d",
       (unsigned int) sizePoses(), (unsigned int) sizeTimeDiffs());
    return false;
  }
  return true;
}

  1. 设置起点并且固定（不允许被优化）。
  2. 设置到目标一条直线上平均间隔的点作为初始位姿。
  3. 设置初始的时间间隔序列。
  4. 设置终点并固定（不允许被优化）。

大致流程
注意： dt的单位是秒，不是时间戳，是路径点之间的时间差。所有点都插入 pose_vec_，但是除起点外的timestep都插入 timediff_vec_

热启动 updateAndPruneTEB

更新和改变轨迹，即根据现在的位置更新起点（vector中的第一个点），改变终点。调用 teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples);

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
46
// new_start new_goal 的根源都在 transformed_plan
void TimedElasticBand::updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, 
  boost::optional<const PoseSE2&> new_goal,   int min_samples)
{
  // first and simple approach: change only start confs(and virtual start conf for inital velocity)
  // TEST if optimizer can handle this "hard" placement
  if ( new_start && sizePoses()>0 )
  {
    // find nearest state (using L2-norm) in order to prune the trajectory
    // (remove already passed states)
    double dist_cache = (new_start->position()- Pose(0).position()).norm();
    double dist;
    // satisfy min_samples, otherwise max 10 samples
    int lookahead = std::min<int>( sizePoses()-min_samples, 10); 

    int nearest_idx = 0;
    for (int i = 1; i<=lookahead; ++i)
    {
      dist = (new_start->position()- Pose(i).position() ).norm();
      if (dist < dist_cache)
      {
        dist_cache = dist;
        nearest_idx = i;
      }
      else break;
    }
    // prune trajectory at the beginning (and extrapolate 
    //sequences at the end if the horizon is fixed )
    if (nearest_idx>0)
    {
      // nearest_idx is equal to the number of samples to be removed (since it counts from 0 )
      // WARNING delete starting at pose 1, and overwrite the original pose(0) with 
      // new_start, since Pose(0) is fixed during optimization!

      // delete first states such that the closest state is the new first one
      deletePoses(1, nearest_idx);  
      deleteTimeDiffs(1, nearest_idx); // delete corresponding time differences
    }
    // update start
    Pose(0) = *new_start;
  }
  if (new_goal && sizePoses()>0)
  {
    BackPose() = *new_goal;
  }
};

当机器人位置变化时，初始轨迹是删去已经走过的，保留后续的。

作用：

  1. 取当前位置为起点，删去已经走过的轨迹，并重新设定终点。
  2. 对于位姿与时间序列，删减去新起点之前的位姿和时间，也就是删除两个容器中旧的元素
  3. 将新的终点加入序列
(十) optimizeTEB 1 流程和 autoResize|路径规划TEB算法

optimizeTEB 的流程就是循环几次下列操作（多次图优化）：

  1. teb_.autoResize() 在外循环里
  2. buildGraph函数;
  3. optimizeGraph 内循环就是g2o的optimize函数的参数
  4. computeCurrentCost函数（目前的图优化的花费，只在最后一次外循环时进行一次）
  5. clearGraph函数;
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
46
47
48
bool TebOptimalPlanner::optimizeTEB( int  iterations_innerloop, 
	   int  iterations_outerloop,  bool compute_cost_afterwards,
     double  obst_cost_scale,    double viapoint_cost_scale, 
     bool  alternative_time_cost )
{
  if (cfg_->optim.optimization_activate == false) 
      return false;

  bool success = false;
  optimized_ = false;
  // ROS源码里是 1
  double weight_multiplier = 2.0;
  // TODO: we introduced the non-fast mode with the support of dynamic obstacles
  // ( which leads to better results in terms of x-y-t homotopy planning )
  //  但此模式尚未完全测试
  // 目前保持legacy fast mode为默认
  bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; // false
  
  for(int i=0; i<iterations_outerloop; ++i)
  {
    if (cfg_->trajectory.teb_autosize)   // 设置为 true
    {
      teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, 
        cfg_->trajectory.min_samples,  cfg_->trajectory.max_samples, fast_mode);
    }
    success = buildGraph(weight_multiplier);
    if (!success) 
    {
        clearGraph();
        return false;
    }
    success = optimizeGraph(iterations_innerloop, false);
    if (!success) 
    {
        clearGraph();
        return false;
    }
    optimized_ = true;
    // compute cost vec only in the last iteration
    if (compute_cost_afterwards &&  i==iterations_outerloop-1) 
      computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
      
    clearGraph();
    // 根据目前配置，变成 4 了
    weight_multiplier *= cfg_->optim.weight_adapt_factor;
  }
  return true;
}

optimization_activate

  1. 外部循环通过调用autoResize()来根据时间分辨率调整轨迹，应根据CPU的控制速率要求来定

  2. 内部循环调用optimizeGraph()进行优化，内部循环的时间经过实验2-6次足够

  3. 最后调用computeCurrentCost()计算轨迹花费，将图中所有边的error的平方和构成cost

autoResize

一般不要用快速模式，也就是要进行100次外循环

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
46
47
48
49
50
// iterate through all TEB states and add/remove states
void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, 
            int min_samples,  int max_samples,  bool fast_mode)
{
  bool modified = true;
  // 确保不会get stuck in some oscillation, 因此最大100此循环
  for (int rep = 0; rep < 100 && modified; ++rep) 
  {
    modified = false;
    // TimeDiff connects Point(i) with Point(i+1)
    // TimeDiff   return timediff_vec_.at(index)->dt();
    for(int i=0; i < sizeTimeDiffs(); ++i) 
    {
      if(TimeDiff(i) > dt_ref + dt_hysteresis && sizeTimeDiffs()<max_samples)
      {
        //ROS_DEBUG("teb_local_planner: autoResize() inserting new bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs()   );
        double newtime = 0.5 * TimeDiff(i);
        TimeDiff(i) = newtime;
        // 时间差太大，插入一个元素
        insertPose(i+1, PoseSE2::average(Pose(i),Pose(i+1)) );
        insertTimeDiff(i+1, newtime);

        modified = true;
      }
      // only remove samples if size is larger than min_samples.
      else if(TimeDiff(i) < dt_ref - dt_hysteresis && sizeTimeDiffs()>min_samples)
      {
        //ROS_DEBUG("teb_local_planner: autoResize() deleting bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs()   );
        if(i < ((int)sizeTimeDiffs()-1))
        {
          // 时间差太小，合并 i 到 i+1
          TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i);
          deleteTimeDiff(i);
          deletePose(i+1);
        }
        else
        { // last motion should be adjusted, shift time to the interval before
          // 合并 i 到 i-1
          TimeDiff(i-1) += TimeDiff(i);
          deleteTimeDiff(i);
          deletePose(i);
        }

        modified = true;
      }
    }
    // 如果是快速模式则结束，也就是rep<100不起作用
    if (fast_mode)    break;
  }
}

这里其实是动态对initTrajectoryToGoal中建立的弹性带进行调整，不能让位姿点和deltT太密集，也不能太稀疏。min_samples和max_samples就是一组限制条件，dt_ref 和 dt_hysteresis是另一组

ROS自带的恢复行为|路径规划move_base分析

原理图

所有的recovery behaviors都必须实现nav_core::RecoveryBehavior定义的接口(纯虚函数)。目前有类ClearCostmapRecovery, MoveSlowAndClear, RotateRecovery， RotateRecovery一般不用

每个恢复行为类主要实现两个函数，一个负责初始化，另一个负责执行恢复行为。

1
2
virtual void initialize()
virtual void runBehavior() = 0;

参数

恢复行为在move_base_params.yaml中定义，如果不定义，会使用默认的配置。MoveBase构造函数中的使用：

1
2
if(!loadRecoveryBehaviors(private_nh))
      loadDefaultRecoveryBehaviors();

如果加载自己的恢复行为不成功，就加载系统默认的。 如果用默认参数，则是按顺序加载了①conservative_clear, ②rotate, ③aggressive_clear, ④rotate. ①/③都是ClearCostmapRecovery类，负责清理机器人一定范围外的costmap上的数据，区别在于③保留的范围更小，清理的更多。恢复行为是按照列表顺序调用的，当①②无效后，才会执行③，清理更多区域。 默认配置中，aggressive_reset清除的范围是4 * local_costmap/circumscribed_radius以外


我的配置是这样的：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'aggressive_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
conservative_reset:
  reset_distance: 3.0
  layer_names: 
     - obstacles
    
aggressive_reset:
  reset_distance: 0.0
  layer_names:
     - obstacles

函数中的变量recovery_behaviors_插入的就是conservative_reset和aggressive_reset。

两个恢复行为都是ClearCostmapRecovery类，相关参数:

  • reset_distance (double, default: 3.0)指定距离机器人半径几米的区域之外的障碍在恢复代价地图为静态地图时候将被清除。
  • layer_names (std::vector, default:”obstacles”)哪一层的信息将会被清除


还有两个重要参数：

  • oscillation_distance (double, default: 0.5) 来回震荡的距离，超过一定的时间没有移动足够的距离就会进行recovery behavior。移动达到了这个距离，就会重置oscillation_timeout的计时器

  • oscillation_timeout (double, default: 0.0) 在执行恢复行为前允许的振荡时间，0代表无穷的时间

我的小车不允许有一些奇怪的行为，比如振荡。所以不用rotate recovery，oscillation_distance设置为0.05也是为了避免振荡

状态机

Movebase有三个状态：

  • PLANNING：全局规划中，尚未得到全局路径
  • CONTROLLING：全局规划完成，进入局部规划
  • CLEARING：恢复行为

当 ①全局规划失败 ②局部规划失败 ③机器人震荡时会进入到恢复行为。

三种情况对应的恢复行为起因

1
2
3
4
5
6
enum RecoveryTrigger
{
  PLANNING_R,      // 全局规划失败，即无法算出 plan
  CONTROLLING_R,  // 局部规划失败，从而无法得到速度命令
  OSCILLATION_R   // 机器人在不断抖动，长时间被困在一小片区域
};

恢复行为的状态机.png

日志：

1
2
3
4
5
6
7
move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
Clearing costmap to unstuck robot (3.000000m).

move_base_recovery,state machine is CLEARING, Executing behavior 1 of 2
Clearing costmap to unstuck robot (0.000000m).

Aborting because a valid plan could not be found. Even after executing all recovery behaviors

RotateRecovery::runBehavior()

只是在原地逆时针旋转180度，这样costmap就会自己更新，mark或者clear一点的值，有没有路是局部代价地图在转一圈过程中发现的。

这里的目标是转过180度，将当前角度与目标180的偏差记录在distleft中，并通过这个偏差产生角速度的控制量，具体为 `vel = sqrt(2 * acc_lim_th * distleft)` ，并且要确保这个速度在角速度阈值之内，发布这个角速度，并循环检测偏差是否满足tolerance范围，当符合时退出。

按默认情况，它先清理周围一定范围以外的costmap（障碍层），接下来重新执行规划，若不奏效，则旋转180度，再执行规划。交替两次后，已转过360度，若还是没能排除故障，则恢复行为失败，关闭Movebase规划器。

ClearCostmapRecovery::runBehavior()

它将距离机器人 一定距离的正方形区域外的 代价地图重置，理解不了看看参数reset_distance就明白了。 ClearCostmapRecovery自身不会让机器人运动

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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
void ClearCostmapRecovery::runBehavior(){
  // 省略：判断是否初始化，两个代价地图是否为空，否则return
  // reset_distance_ 就是定义的参数
  clear_costmap_log.warn("Clearing costmap to unstuck robot (%fm).", reset_distance_);
  clear(global_costmap_);
  clear(local_costmap_);
}

void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap)
{
  std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();

  tf::Stamped<tf::Pose> pose;
  // 在代价地图的global坐标系中的坐标 x  y
  if(!costmap->getRobotPose(pose)){
    ROS_ERROR("Cannot clear map because pose cannot be retrieved");
    return;
  }
  double x = pose.getOrigin().x();
  double y = pose.getOrigin().y();
  // 遍历代价地图的每一层，转为costmap_2d::Layer，获取名称
  for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp)
  {
    boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
    std::string name = plugin->getName();
    int slash = name.rfind('/');
    if( slash != std::string::npos ){
        name = name.substr(slash+1);
  }
    if(clearable_layers_.count(name)!=0){
      boost::shared_ptr<costmap_2d::CostmapLayer> costmap;
      costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
      clearMap(costmap, x, y);
    }
  }
}

void ClearCostmapRecovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap, 
                                    double pose_x, double pose_y)
{
  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
 
  double start_point_x = pose_x - reset_distance_ / 2;
  double start_point_y = pose_y - reset_distance_ / 2;
  double end_point_x = start_point_x + reset_distance_;
  double end_point_y = start_point_y + reset_distance_;

  int start_x, start_y, end_x, end_y;
  costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
  costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
  // 获取字符格式的代价地图
  unsigned char* grid = costmap->getCharMap();
  for(int x=0; x<(int)costmap->getSizeInCellsX(); x++)
  {
    bool xrange = x>start_x && x<end_x;   
    for(int y=0; y<(int)costmap->getSizeInCellsY(); y++)
    {
    	// 注意是不处理 reset_distance_正方形范围内的代价值
      if(xrange && y>start_y && y<end_y)
        continue;
      int index = costmap->getIndex(x,y);
      // 范围外的代价值重置为 NO_INFORMATION
      if(grid[index]!=NO_INFORMATION){
        grid[index] = NO_INFORMATION;
      }
    }
  }
  double ox = costmap->getOriginX(), oy = costmap->getOriginY();
  double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();

  costmap->addExtraBounds(ox, oy, ox + width, oy + height);
  return;
}

MoveSlowAndClear

这个使用较少。 它实现了缓慢移动的恢复行为，清理 costmap后什么都不管，按照前进速度和转角速度走。从代码里可以看到，根据指定的距离，这是通过先清除全局 costmap 跟局部 costmap 一圈的 obstacle layer 的障碍，然后直接发指令实现的。由于只清除了 obstacle layer ，其实 static layer 的障碍还在，而且这里清不清除跟发指令关系不大，该撞上去的还是会撞的，相当于闭着眼睛往前走。

禁止所有恢复行为造成的问题

不如自己不定义，还会使用默认的参数，所以将下面代码去掉:

1
2
if(!loadRecoveryBehaviors(private_nh))
      loadDefaultRecoveryBehaviors();

结果发现平时正常导航的情况居然出错了

1
2
3
4
found_legal is 0
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan 
[ERROR] 17:51:42.074 (): Aborting because a valid plan could not be found. Even after executing all recovery behaviors

并不是每次的导航都会出错，这要看全局路径，全局路径部分的bool found_legal = planner_->calculatePotentials有时会返回0，也就是找不到全局路径，此时会触发恢复行为。所以如果全禁止，会直接 Abort

正常的日志是这样的:

1
2
3
4
5
6
7
8
9
10
Received move_base goal (x: 62.766, y: -1.474, yaw deg: -5, yaw rad: -0.09)
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan 
[INFO]  move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
[ WARN] Clearing costmap to unstuck robot (0.000000m).
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan 
[ INFO]  move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
[ WARN] Clearing costmap to unstuck robot (0.000000m).
[ INFO] 到达目标

也就是获取不到全局路径时，会很快执行一次恢复行为，clear costmap，然后又找到了路径，这个过程之快，几乎看不到。

参考： 导航recovery机制

(七) Teb Optimal的路径规划|路径规划TEB算法
1
2
3
4
5
6
7
8
9
10
11
12
13
// 禁止配置参数的更改 during the following optimization step
boost::mutex::scoped_lock  cfg_lock(cfg_.configMutex() );
bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
if (!success)
{
    planner_->clearPlanner(); // force reinitialization for next time
    ROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting.");
    
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel;
    return false;
}

clearPlanner()

不会停止规划，而是强制下次重新初始化planner，要停止继续规划，只能return。 如果本次导航失败或路径不好，出现一大堆TebPose缠绕，在下一次导航时，它们可能还在，要消除就需要clearPlanner();
下次导航前，需要clearPlanner.png


实际中用的是Homotopy Planner，但是它太复杂了，我们先看TebOptimal Planner，前者取的其实是 best Teb Optimal Planner 
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
46
47
48
bool TebOptimalPlanner::plan(
            const std::vector<geometry_msgs::PoseStamped>&  initial_plan, 
            const geometry_msgs::Twist*  start_vel, 
            bool  free_goal_vel )
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
  // 每次收到新目标时执行，然后pose size都是3
  if (!teb_.isInit() )
  {
      teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, 
      cfg_->robot.max_vel_theta,   cfg_->trajectory.global_plan_overwrite_orientation,
      cfg_->trajectory.min_samples,  cfg_->trajectory.allow_init_with_backwards_motion);
    // 0 个 intermediate samples, dt=1 --> autoResize  will add more samples 
    // before first optimization
  }
  else    // 热启动
  {
    PoseSE2 start_(initial_plan.front().pose);
    PoseSE2 goal_(initial_plan.back().pose);
    if (teb_.sizePoses()>0
        && ( goal_.position() - teb_.BackPose().position()).norm() < 
            cfg_->trajectory.force_reinit_new_goal_dist
        && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < 
            cfg_->trajectory.force_reinit_new_goal_angular ) // actual warm start!

      teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples);

      else // goal is too far away from BackPose, reinit相当于 isInit()为false
      {
          // 当导航较远的goal时，则首先clear轨迹序列，然后重新起点终点插值
          teb_.clearTimedElasticBand();
          teb_.initTrajectoryToGoal( start, goal, 0, cfg_->robot.max_vel_x, 
            cfg_->trajectory.min_samples, 
            cfg_->trajectory.allow_init_with_backwards_motion );
      }
  }
  // 当前从里程计获取的速度
  if (start_vel)
      // 赋值给 std::pair<bool, geometry_msgs::Twist> vel_start_; 
      setVelocityStart(*start_vel);   // start pose的速度
  if (free_goal_vel)            // 一般为 false
      setVelocityGoalFree();   // 这是唯一调用此函数的地方，不看了
  else
    // reactivate and 使用之前设置的速度 ( 如果未修改则是0 )
    vel_goal_.first = true; 

  return optimizeTEB(cfg_->optim.no_inner_iterations,  cfg_->optim.no_outer_iterations);
}

isInit()检查trajectory是否初始化: bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}

1
2
3
4
5
6
7
8
9
10
11
12
13
// 清空 timediff_vec_ 和 pose_vec_， 之后 isInit返回 false
void TimedElasticBand::clearTimedElasticBand()
{
  for (PoseSequence::iterator pose_it = pose_vec_.begin(); 
             pose_it != pose_vec_.end(); ++pose_it)
      delete *pose_it;
    pose_vec_.clear();
  
  for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin(); 
           dt_it != timediff_vec_.end();   ++dt_it )
      delete *dt_it;
    timediff_vec_.clear();
}

关于变量pose_vec_和timediff_vec_，看下篇文章


plan 函数涉及参数 max_vel_x, min_samples, force_reinit_new_goal_dist, allow_init_with_backwards_motion, free_goal_vel

TebOptimalPlanner::plan()主要是根据输入的初始路径初始化或更新时间弹性带(轨迹)的初始状态，设置了轨迹起始点以及速度加速度的约束， 最后调用 optimizeTEB() 函数：这里是核心中的核心，最关键的就是建图buildGraph和优化OptimizeGraph两个步骤

将轨迹优化问题构建成了一个g2o图优化问题,并通过g2o中关于大规模稀疏矩阵的优化算法解决，涉及到构建超图(hyper-graph)，简单来说 将机器人位姿和时间间隔描述为节点(vertex)，目标函数以及约束函数作为边(edge)。 graph中，每一个约束都为一条边，并且每条边允许连接的节点的数目是不受限制的。

建图是将路径的一系列约束加进去，比如与障碍物保持一定距离，速度、加速度限制、时间最小约束、最小转弯半径约束、旋转方向约束等，这些都转换成了代价函数放在g2o中，g2o优化时会使这些代价函数达到最小。

TebOptimalPlanner 的初始化

在TebLocalPlannerROS::initialize有这样一段

1
2
3
4
5
6
7
8
9
10
if (cfg_.hcp.enable_homotopy_class_planning)
{
   // 省略
}
else
{
  planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, 
     &obstacles_,  robot_model,  visualization_,  &via_points_)  );
  ROS_INFO("Parallel planning in distinctive topologies disabled.");
}

TebOptimalPlanner的构造函数就一句initialize(cfg, obstacles, robot_model, visual, via_points);，实际是:
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
void TebOptimalPlanner::initialize(const TebConfig& cfg, 
      ObstContainer* obstacles,
      RobotFootprintModelPtr  robot_model, 
      TebVisualizationPtr  visual, 
      const ViaPointContainer* via_points)
{    
  // init optimizer (set solver and block ordering settings)
  optimizer_ = initOptimizer();
  cfg_ = &cfg;
  obstacles_ = obstacles;
  robot_model_ = robot_model;
  via_points_ = via_points;
  cost_ = HUGE_VAL;
  prefer_rotdir_ = RotType::none;
  setVisualization(visual);
  // 在上面的 setVelocityStart 里赋值
  vel_start_.first = true;
  vel_start_.second.linear.x = 0;
  vel_start_.second.linear.y = 0;
  vel_start_.second.angular.z = 0;

  vel_goal_.first = true;
  vel_goal_.second.linear.x = 0;
  vel_goal_.second.linear.y = 0;
  vel_goal_.second.angular.z = 0;
  initialized_ = true;
}

initOptimizer()函数在(十) optimizeTEB 2 图优化的过程

(六) 静态障碍和自定义障碍|路径规划TEB算法

对应的片段:

1
2
3
4
5
6
7
8
9
10
11
12
// clear currently existing obstacles
obstacles_.clear();   // 初始化时 obstacles_.reserve(500);
// 用代价地图信息更新 obstacle container 或者
// 用costmap_converter提供的polygons 
if (costmap_converter_)
  updateObstacleContainerWithCostmapConverter();
else
  updateObstacleContainerWithCostmap();

// 考虑custom obstacles (must be called after other updates,
//  since the container is not cleared  )
updateObstacleContainerWithCustomObstacles();

obstacles_来源于两部分：

  1. 来自代价地图updateObstacleContainerWithCostmap() 或者 CostmapConverter 的updateObstacleContainerWithCostmapConverter()

  2. 来自用户发布的障碍物消息updateObstacleContainerWithCustomObstacles()

分别由参数include_costmap_obstacles 和 costmap_converter_plugin决定，还有动态障碍include_dynamic_obstacles。 之后又对应buildGraph中的AddEdgesObstacles与AddEdgesDynamicObstacles

其实目前用的是costmap_converter

updateObstacleContainerWithCostmap

获得当前朝向的单位向量

1
2
3
4
Eigen::Vector2d  PoseSE2::orientationUnitVec() const
{
  return Eigen::Vector2d(std::cos(_theta), std::sin(_theta) );
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
// Add costmap obstacles if desired
if (cfg_.obstacles.include_costmap_obstacles)
{
  Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec();
  
  for (unsigned int i=0; i<costmap_->getSizeInCellsX()-1; ++i)
  {
    for (unsigned int j=0; j<costmap_->getSizeInCellsY()-1; ++j)
    {
      if (costmap_->getCost(i,j) == costmap_2d::LETHAL_OBSTACLE)
      {
        Eigen::Vector2d obs;
        costmap_->mapToWorld(i,j, obs.coeffRef(0), obs.coeffRef(1));

        // 检查障碍是否 not far behind the robot
        Eigen::Vector2d obs_dir = obs - robot_pose_.position();
        // if(obs_dir.norm() < 1.5)    //改变最大速度约束
        //   cfg_.robot.max_vel_x = 0.3;
        // 如果norm值过大 或者  obs_dir和robot_orient的点乘积小于0， 不算障碍
        if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_.obstacles.costmap_obstacles_behind_robot_dist  )
             continue;
        // 点障碍
        obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
      }   }   }   }

这里是参数include_costmap_obstacles唯一出现的地方。 参数costmap_obstacles_behind_robot_dist默认是1.
函数根据costmap中的障碍物信息更新障碍物容器。在这里，可以找到前方距离障碍物的最短距离。然后根据该距离值调整max_vel_x，这样优化时最大线速度的约束会随着障碍物的远近而调整，在一定程度上会改善避障效果。

自定义障碍

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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
{
  // Add custom obstacles obtained via message
  boost::mutex::scoped_lock l(custom_obst_mutex_);
  // 类型 costmap_converter::ObstacleArrayMsg
  if (!custom_obstacle_msg_.obstacles.empty())
  {
    // We only use the global header to specify the obstacle coordinate system 
    // instead of individual ones
    Eigen::Affine3d obstacle_to_map_eig;
    try 
    {
      tf::StampedTransform  obstacle_to_map;
      tf_->waitForTransform(global_frame_, ros::Time(0),
            custom_obstacle_msg_.header.frame_id, ros::Time(0),
            custom_obstacle_msg_.header.frame_id, ros::Duration(0.5) );
      tf_->lookupTransform(global_frame_, ros::Time(0),
          custom_obstacle_msg_.header.frame_id, ros::Time(0), 
          custom_obstacle_msg_.header.frame_id, obstacle_to_map);
      tf::transformTFToEigen(obstacle_to_map, obstacle_to_map_eig);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s",ex.what());
      obstacle_to_map_eig.setIdentity();
    }
    for (size_t i=0; i<custom_obstacle_msg_.obstacles.size(); ++i)
    {
      if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 && 
          custom_obstacle_msg_.obstacles.at(i).radius > 0 ) // circle
      {
        Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
        obstacles_.push_back(ObstaclePtr(new CircularObstacle( (obstacle_to_map_eig * pos).head(2), custom_obstacle_msg_.obstacles.at(i).radius) )  );
      }
      else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 ) // point
      {
        Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
        obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) )));
      }
      else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 2 ) // line
      {
        Eigen::Vector3d line_start( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
        Eigen::Vector3d line_end( custom_obstacle_msg_.obstacles.at(i).polygon.points.back().x,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.back().y,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.back().z );
        obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2),
                                             (obstacle_to_map_eig * line_end).head(2) ) )  );
      }
      else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.empty())
      {
        ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
        continue;
      }
      else // polygon
      {
        PolygonObstacle* polyobst = new PolygonObstacle;
        for (size_t j=0; j<custom_obstacle_msg_.obstacles.at(i).polygon.points.size(); ++j)
        {
          Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points[j].x,
                               custom_obstacle_msg_.obstacles.at(i).polygon.points[j].y,
                               custom_obstacle_msg_.obstacles.at(i).polygon.points[j].z );
          polyobst->pushBackVertex( (obstacle_to_map_eig * pos).head(2) );
        }
        polyobst->finalizePolygon();
        obstacles_.push_back(ObstaclePtr(polyobst));
      }
      // Set velocity, if obstacle is moving
      if(!obstacles_.empty())
      {
        obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation);
        obstacles_.back()->setDynamic();
      }
    }
  }
}

消息costmap_converter/ObstacleArrayMsg定义：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
std_msgs/Header header    # frame_id 为map
costmap_converter/ObstacleMsg[] obstacles

# costmap_converter/ObstacleMsg 的成员如下
std_msgs/Header header    # frame_id 为map
# Obstacle footprint (polygon descriptions)
geometry_msgs/Polygon polygon
# radius for circular/point obstacles
float64 radius
# Obstacle ID
# Specify IDs in order to provide (temporal) relationships
# between obstacles among multiple messages.
int64 id
# Individual orientation (centroid)
geometry_msgs/Quaternion orientation
# Individual velocities (centroid)
geometry_msgs/TwistWithCovariance velocities

类Obstacle定义在obstacle.h，派生类有PointObstacle, CircularObstacle, LineObstacle,PolygonObstacle

1
2
typedef boost::shared_ptr<Obstacle> ObstaclePtr;
typedef std::vector<ObstaclePtr>  ObstContainer;

(三) 从全局路径获得 transform_plan|路径规划TEB算法

MoveBase::executeCycle() —-> computeVelocityCommands，就是从这个函数开始调用TEB算法，下面转入teb_local_planner_ros.cpp文件中。

开始代码:

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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
// check if plugin initialized，省略
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
cmd_vel.angular.z = 0;
goal_reached_ = false;  
// 修改源码
if(!rotate_to_path_ && new_goal_)
{
    double start_angle = tf::getYaw(path_start_orientation_);
    tf::Stamped<tf::Pose> robot_pose;
    costmap_ros_->getRobotPose(robot_pose);
    double robot_angle = tf::getYaw(robot_pose.getRotation());

    bool result  = pointToPath(start_angle, robot_angle, cmd_vel);
    if(result)
    {
      rotate_to_path_ = true;
      return true;
    }
    return true;
}

// Get robot pose in global frame(map坐标系),转为PoseSE2类型
tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
robot_pose_ = PoseSE2(robot_pose);

// Get robot velocity from odom
tf::Stamped<tf::Pose> robot_vel_tf;
odom_helper_.getRobotVel(robot_vel_tf);
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation()); 

// prune global plan to cut off parts of the past (spatially before the robot)
pruneGlobalPlan(*tf_, robot_pose, global_plan_);

// Transform global plan to the frame of interest (w.r.t. the local costmap)
std::vector<geometry_msgs::PoseStamped> transformed_plan;
int goal_idx;
tf::StampedTransform tf_plan_to_global;
if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, 
                         transformed_plan, &goal_idx, &tf_plan_to_global))
{
  ROS_WARN("Could not transform the global plan to the frame of the controller");
  return false;
}

// update via-points container 这里是false,不用看了
if (!custom_via_points_active_)
  updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);

// check if global goal is reached
tf::Stamped<tf::Pose> global_goal;
tf::poseStampedMsgToTF(global_plan_.back(), global_goal);
global_goal.setData( tf_plan_to_global * global_goal );
double dx = global_goal.getOrigin().getX() - robot_pose_.x();
double dy = global_goal.getOrigin().getY() - robot_pose_.y();
double delta_orient = g2o::normalize_theta( tf::getYaw(global_goal.getRotation()) - robot_pose_.theta() );
 // 是否达到目标位置
if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance)
position_reanched_ = true;

 // 修改源码
if(position_reached_)
{ 
   double goal_angle = tf::getYaw(goal_orientation_);
   double robot_angle = tf::getYaw(robot_pose.getRotation());

   bool result  = pointToPath(goal_angle, robot_angle, cmd_vel);
   if(result)
   {
      goal_reached_ = true;
      return true;
   }
   return true;
}

if(!goal_reached_)
{
    if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
      && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
      && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0))
    {
      goal_reached_ = true;
      return true;
    }
}
// check if we should enter any backup mode and apply settings
configureBackupModes(transformed_plan, goal_idx);

// Return false if the transformed global plan is empty
if (transformed_plan.empty())
{
  ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
  return false;
}

  • pruneGlobalPlan: 截取全局路径的一部分作为局部规划的初始轨迹，主要是把全局路径中机器人已经经过的位姿删去。取决于参数 dist_behind_robot，机器人身后要保留的距离，单位米，默认1，不能太小，尤其不能小于地图的cellsize。 在调用前后，global_plan_常常不发生变化，所以不必太关注。

  • transformGlobalPlan: 根据local costmap地图大小，将此地图内的全局规划器的规划保存到transformed_plan中。transformed_plan所在的坐标系仍然为global_frames

global_plan_对应的就是话题/move_base/TebLocalPlannerROS/global_plan，它的size和成员一般在规划两三次后就会变，size一般是逐步变小，transformed_plan也跟随变化。

transformGlobalPlan函数之后，transformed_plan开始和global_plan_相等，transformed_plan逐渐取global_plan_的后面部分，此时goal_idx == global_plan.size() - 1。 有时候transformed_plan取global_plan_中间部分，此时goal_idx != global_plan.size() - 1

这三者会出现以下几种情况:

  1. global_plan_ size: 49, transformed_plan size:49, goal_idx: 48 : transformed_plan是从global_plan第1个点开始(id=0), 二者完全相同，goal_idx对应transformed_plan和global_plan_最后一个点

  2. global_plan_ size: 102, transformed_plan size:69, goal_idx: 68: transformed_plan是从global_plan第1个点开始(id=0), 但这次 transformed_plan 取得是 global_plan 前半部分， goal_idx对应transformed_plan最后一个点，

  3. global_plan_ size: 49, transformed_plan size:45, goal_idx: 48: transformed_plan是从global_plan第5个点开始(id=4)，取global_plan后半部分， goal_idx对应transformed_plan和global_plan_最后一个点

  4. global_plan size: 98, transformed_plan size:69, goal_idx: 73: transformed_plan是从global_plan第6个点开始(id=5), 也就是取中间部分，goal_idx对应transformed_plan最后一个点。 goal_idx != transformed_plan.size()-1

goal_idx 是 global_plan 中的id，它对应的点一定和 transformed_plan 最后一个点相同，所以后面才可以赋值给goal_point和robot_goal_

(二) 算法的初始化|路径规划TEB算法

MoveBase中加载局部路径规划器

1
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);

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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
  if(!initialized_)
  {
    // create Node Handle with name of plugin (as used in move_base for loading)
    ros::NodeHandle nh("~/" + name);
	        
    // get parameters of TebConfig via the nodehandle and override the default config
    cfg_.loadRosParamFromNodeHandle(nh);  // TebConfig  cfg_;
    // nh.param("trapped_threshold", trapped_threshold_, 20);
    nh.param("filter_back_velocity", filter_back_velocity_, true); 
    // nh.param("setting_acc", setting_acc_, 1.5);     

    obstacles_.reserve(500);    // ObstContainer  obstacles_;
        
    // 在TebVisualization构造函数里,注册话题 global_plan, local_plan,teb_poses, teb_markers, teb_feedback
    visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); 
        
    //创建robot footprint/contour model for optimization，对5种类型分别处理
    // 点类型最简单，返回新建的PointRobotFootprint共享指针
    // 对于polygon类型，只对 Point2dContainer vertices_;赋值
    RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);
    
    // 根据参数，选择创建哪种 planner
    if (cfg_.hcp.enable_homotopy_class_planning)
    {
      planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies enabled.");
    }
    else
    {
      planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies disabled.");
    }
    // init other variables
    tf_ = tf;   // 在 move_base_node.cpp中最初定义
    // 这个就是 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    costmap_ros_ = costmap_ros;
    // locking should be done in MoveBase
    costmap_ = costmap_ros_->getCostmap();

    // 创建 CostmapModel 共享指针
    costmap_model_ = boost::make_shared<base_local_planner::CostmapModel>(*costmap_);

    // 就是return global_frame, 实际是 "map"
    global_frame_ = costmap_ros_->getGlobalFrameID();
    cfg_.map_frame = global_frame_; // 更新cf_的成员

    // 代价地图的local frame, 实际是 "base_footprint"
    robot_base_frame_ = costmap_ros_->getBaseFrameID();

    //Initialize a costmap to polygon converter
    if (!cfg_.obstacles.costmap_converter_plugin.empty())
    {
      try
      {
        costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin);
        std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin);
        // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
        boost::replace_all(converter_name, "::", "/");
        costmap_converter_->setOdomTopic(cfg_.odom_topic);
        costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));
        costmap_converter_->setCostmap2D(costmap_);
        
        costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread);
        ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded.");        
      }
      catch(pluginlib::PluginlibException& ex)
      {
        ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treated as point obstacles. Error message: %s", ex.what());
        costmap_converter_.reset();
      }
    }
    else 
      ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");
  
    // 类型是 vector<geometry_msgs::Point>
    footprint_spec_ = costmap_ros_->getRobotFootprint();
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
    // 更新cf_的成员
    cfg_.robot.robot_inscribed_radius =  robot_inscribed_radius_;
    cfg_.robot.robot_circumscribed_radius = robot_circumscribed_radius; 
    
    // init the odom helper to receive the robot's velocity from odom messages
    odom_helper_.setOdomTopic(cfg_.odom_topic);

    // setup dynamic reconfigure
    dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(nh);
    dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);
    
    // validate optimization footprint and costmap footprint
    // 对于point类型，第1个参数是0，要求最好第1项+第3项 > 第2项
    // 否则 Infeasible optimziation results might occur frequently
    validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

    // custom obstacles的回调函数，话题 /move_base/TebLocalPlannerROS/obstacles
    custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

    // custom via-points的回调，话题 /move_base/TebLocalPlannerROS/via_points
    via_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this);
    // queue_follow 所用，话题 /rectangle_distance
    front_dist_sub_ = nh.subscribe("/rectangle_distance", 1, &TebLocalPlannerROS::frontDistanceCB, this);
    // 新添加
    footprint_sub_ = nh.subscribe("/footprint", 10, &TebLocalPlannerROS::footprintCB, this);

    
    ros::NodeHandle nh_move_base("~");
    double controller_frequency = 5;
    // 实际配置是 30
    nh_move_base.param("controller_frequency", controller_frequency, controller_frequency);
    // initialize failure detector，Detect if the robot got stucked
    failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency));
    // 初始化，返回一个很大的数
    min_distance_custom_obstacle = std::numeric_limits<double>::max();
    
    // set initialized flag
    initialized_ = true;
    ROS_DEBUG("teb_local_planner plugin initialized.");
  }
  else
    ROS_WARN("teb_local_planner has already been initialized, doing nothing.");
}

关于RobotFootprintModelPtr，看TEB中的footprint模型

calculateMinAndMaxDistances函数和RobotFootprintModelPtr说明同时使用了两种footprint模型