TEB代码详解

前言

最近为了去根据自己的需求爆改一下TEB所以一直在学习TEB的源码，本文将从源码中学习到的一些东西详细的记录一下，其中最主要的肯定还是computeVelocityCommands这个函数，这个函数的目的是计算速度，但是内部引用的函数几乎囊括了整份代码，可以说算是核心中的核心了。这篇文章的篇幅应该会比较长，因为很多地方会记录的比较细一点，所以还请读者耐心读完，若有理解不当之处，还请大家批评指正。闲话不多说，下面进入正题。

源码分析

bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
  std::string dummy_message;
  geometry_msgs::PoseStamped dummy_pose;
  geometry_msgs::TwistStamped dummy_velocity, cmd_vel_stamped;
  uint32_t outcome = computeVelocityCommands(dummy_pose, dummy_velocity, cmd_vel_stamped, dummy_message);
  cmd_vel = cmd_vel_stamped.twist;
  return outcome == mbf_msgs::ExePathResult::SUCCESS;
}

uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose,
                                                     const geometry_msgs::TwistStamped& velocity,
                                                     geometry_msgs::TwistStamped &cmd_vel,
                                                     std::string &message)
{
  // check if plugin initialized
  //判断是否初始化插件
  if(!initialized_)
  {
    ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner");
    message = "teb_local_planner has not been initialized";
    return mbf_msgs::ExePathResult::NOT_INITIALIZED;
  }

  static uint32_t seq = 0;
  cmd_vel.header.seq = seq++;
  cmd_vel.header.stamp = ros::Time::now();
  cmd_vel.header.frame_id = robot_base_frame_;
  cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
  goal_reached_ = false;  
  
  // Get robot pose
  //获取机器人当前位姿
  geometry_msgs::PoseStamped robot_pose;
  costmap_ros_->getRobotPose(robot_pose);
  robot_pose_ = PoseSE2(robot_pose.pose);
    
  // Get robot velocity
  //获取机器人线速度与角速度
  geometry_msgs::PoseStamped robot_vel_tf;
  odom_helper_.getRobotVel(robot_vel_tf);
  robot_vel_.linear.x = robot_vel_tf.pose.position.x;
  robot_vel_.linear.y = robot_vel_tf.pose.position.y;
  robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);

起头这块没啥好说的，主要就是初始化一些变量，然后获取机器人的线速度与角速度。

pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);

接下就是修建全局路径，就比如用全局算法规划一条从初始点到目标点的轨迹，此时小车已经走过这个轨迹的一半了，所以前面的很多轨迹就没必要留着了，就给它裁剪掉，这个可以优化后面TEB对一些路径点处理的效率，减少一些重复计算。传入的参数第一个是tf，第二个刚刚获取的机器人的当前位姿，第三个是全局路径，第四个是我们自定义的参数global_plan_prune_distance即修建全局路径的距离保留多少已走过的轨迹（默认值为1）。下面先说一下这个全局路径这个参数我们是哪里获取的，因为其他的我们都知道他的来路，唯独这一个参数。在computeVelocityCommands这个函数的上面还有一个函数叫setPlan。

bool TebLocalPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{
  // check if plugin is initialized
  if(!initialized_)
  {
    ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner");
    return false;
  }

  // store the global plan
  global_plan_.clear();
  global_plan_ = orig_global_plan;

  // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan.
  // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step.  
            
  // reset goal_reached_ flag
  goal_reached_ = false;
  
  return true;
}

这部分的代码作用就是设置TEB要跟随的一个路径，因为我们有全局规划，TEB不可能毫无目的的走的，TEB要做的是在全局轨迹的基础上考虑一些动力学约束还有障碍物等因素生成一个局部规划路径，所以我们这部分的作用就是把我们规划的全局轨迹传给TEB，所以在这部分函数的后面我们将全局路径赋给了global_plan_，也就是刚刚传给pruneGlobalPlan的参数。

接下来再进入到pruneGlobalPlan这个函数：

bool TebLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot)
{
  if (global_plan.empty())
    return true;
  
  try
  {
    // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times)
    geometry_msgs::TransformStamped global_to_plan_transform = tf.lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, ros::Time(0));
    geometry_msgs::PoseStamped robot;
    tf2::doTransform(global_pose, robot, global_to_plan_transform);
    // dist_thresh_sq 为 离车后面距离的平方，下面与车和it位置的平方对比
    double dist_thresh_sq = dist_behind_robot*dist_behind_robot;
    
    // iterate plan until a pose close the robot is found
    //定义两个迭代器it 和 erase_end ,将全局路径的起点赋给it，it 赋给 erase_end 
    std::vector<geometry_msgs::PoseStamped>::iterator it = global_plan.begin();
    std::vector<geometry_msgs::PoseStamped>::iterator erase_end = it;
    //从初始点遍历，若it与车的距离小于dist_behind_robot，则it取下面一个点，
    //直至it进入dist_behind_robot范围，将 it 的位置赋给 erase_end
    while (it != global_plan.end())
    {
      double dx = robot.pose.position.x - it->pose.position.x;
      double dy = robot.pose.position.y - it->pose.position.y;
      double dist_sq = dx * dx + dy * dy;
      if (dist_sq < dist_thresh_sq)
      {
         erase_end = it;
         break;
      }
      ++it;
    }
    //若erase_end到路径终点，返回false
    if (erase_end == global_plan.end())
      return false;
    //若erase_end的位置不等于全局路径的起点，就裁减全局路径起点到erase_end之间的路径
    if (erase_end != global_plan.begin())
      global_plan.erase(global_plan.begin(), erase_end);
  }
  catch (const tf::TransformException& ex)
  {
    ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what());
    return false;
  }
  return true;
}

先是一个判断，如果全局路径为空的话就是返回true，否则就执行接下来的操作：

首先是一个TF坐标转换，将机器人在全局坐标系中的姿态转换到规划路径的坐标系中，以便进行路径规划和跟踪。

然后定义一个距离阈值dist_thresh_sq，为车后方路径（即global_plan_prune_distance：修建全局路径的距离）的平方，再定义两个迭代器it和erase_end，两者的初始值都是全局路径的起点。

接下来进入一个循环，从全局路径的初始点遍历，定义变量dist_sq为机器人与it点距离的平方，直到这个距离小于距离阈值dist_thresh_sq，将erase_end赋给it，break。如果erase_end等于全局路径终点位置，则return false，如果erase_end不等于全局路径的起始点，就将全局路径起点和erase_end中间的部分裁剪掉。这样就能保证机器人已走过的距离一直小于自定义global_plan_prune_distance的值。

// Transform global plan to the frame of interest (w.r.t. the local costmap)
  std::vector<geometry_msgs::PoseStamped> transformed_plan;
  int goal_idx;
  geometry_msgs::TransformStamped tf_plan_to_global;
  //max_global_plan_lookahead_dist：考虑全局规划子集的最大长度（受局部代价地图大小和内部参数dist_threshold=0.85限制）
  //transformGlobalPlan（）：选取全局路径的点做局部路径规划
  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");
    message = "Could not transform the global plan to the frame of the controller";
    return mbf_msgs::ExePathResult::INTERNAL_ERROR;
  }

回到computeVelocityCommands中，接下来是上面这部分的代码。这部分代码的主要作用是将全局路径转换到局部代价地图的坐标下，并从中选择合适的点进行局部路径规划。这部分主要是调用了函数transformGlobalPlan（），其中有个自定义参数max_global_plan_lookahead_dist传进去，这个参数的主要目的就是最多前瞻多长距离的全局轨迹进行选取点来做局部规划，首先他一定是受局部代价地图约束的，因为进入到局部代价地图的那一部分我们才会考虑进行局部规划，超出的局部代价地图的路径TEB暂时是不会管的，所以max_global_plan_lookahead_dist设置的已经越过局部代价地图了，让他去规划一些位置的点显然是不合适的。这个参数还受dist_threshold的限制，我们接下来继续说。

bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
                  const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length,
                  std::vector<geometry_msgs::PoseStamped>& transformed_plan, int* current_goal_idx, geometry_msgs::TransformStamped* tf_plan_to_global) const
{
  // this method is a slightly modified version of base_local_planner/goal_functions.h

  const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

  transformed_plan.clear();

  try 
  { //如果全局路径为空，则输出错位并返回false
    if (global_plan.empty())
    {
      ROS_ERROR("Received plan with zero length");
      *current_goal_idx = 0;
      return false;
    }
    //获取从规划坐标系到全局坐标系的变换
    // get plan_to_global_transform from plan frame to global_frame
    geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp,
                                                                                  plan_pose.header.frame_id, ros::Duration(cfg_.robot.transform_tolerance));
    //将机器人的当前位置（global_pose=全局坐标系下的位置）转换到局部规划所在的坐标系下
    //let's get the pose of the robot in the frame of the plan
    geometry_msgs::PoseStamped robot_pose;//robot_pose: 将要被计算出来的机器人在规划坐标系下的位置
    tf.transform(global_pose, robot_pose, plan_pose.header.frame_id);

先看transformGlobalPlan这个函数的前面一小部分，这一部分先是将全局路径的第一个点位信息赋给plan_pose这个参数，目的是局部规划需要知道自己从哪个地方开始要进行对路径的优化，plan_pose就是局部规划的起点。接着将已经转化的路径清空。然后下面进行一个判断：若全局路径为空的话，就return false。后面又是两次坐标的转换，第一次转换是获取局部规划坐标系到全局坐标系的转换，第二个转换是获取在局部规划坐标系下机器人的位姿。

 //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);
    //计算出的阈值乘0.85，即考虑代价地图的85%范围，以确保路径规划时，机器人的动作不会过于接近代价地图的边界，从而避免潜在的安全问题                             
    dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
                           // located on the border of the local costmap
    //通过这个距离阈值，teb_local_planner能够筛选出那些距离机器人足够近，且在成本地图范围内的全局路径点，
    //用于后续的局部路径规划和优化。这有助于提高路径规划的有效性和安全性。

    int i = 0;//i用于存储最近点的索引,初始化为0
    double sq_dist_threshold = dist_threshold * dist_threshold;//sq_dist_threshold是距离阈值的平方，用于比较距离
    double sq_dist = 1e10;//sq_dist初始化为一个非常大的数（1e10），用于存储当前最小距离的平方
    
    //we need to loop to a point on the plan that is within a certain distance of the robot
    bool robot_reached = false;//用于标记是否找到局部最小值
    //遍历全局路径的所有点，计算这些点与当前位置距离的平方，找到全局路径中与机器人最近的位置点
    for(int j=0; j < (int)global_plan.size(); ++j)
    {
      double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x;
      double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y;
      double new_sq_dist = x_diff * x_diff + y_diff * y_diff;
      //若车到达目的地并且是找不到离机器人更近的点了则break
      if (robot_reached && new_sq_dist > sq_dist)
        break;

      if (new_sq_dist < sq_dist) // find closest distance
      {
        sq_dist = new_sq_dist;
        i = j;
        if (sq_dist < 0.05)      // 2.5 cm to the robot; take the immediate local minima; if it's not the global
          robot_reached = true;  // minima, probably means that there's a loop in the path, and so we prefer this
      }
    }

这部分代码的作用是计算一个距离阈值（dist_threshold），这个阈值用于确定哪些全局路径规划点（global_plan）应该被考虑用于局部路径规划。这个阈值是基于代价地图的大小和分辨率来计算的。首先先获取代价地图X和Y边一半的最大值，有两个问题：第一点，为什么是半尺寸？第二点：为什么要取其中的最大值。首先回答第一个问题，因为局部代价地图是个矩形框，它的中心永远是车体的中心，局部代价地图是跟着小车走的，而小车的前面一半是未走过的路径，后面一半是已经走过的路径，我们只需要对前面未走过的路径进行选点加入局部规划就行了。再回答第二个问题：去XY最大值是为了保证这个阈值足够大，可以覆盖代价地图的所有部分。接下来再对dist_threshold这个阈值乘上一个比例系数0.85，即考虑代价地图的85%范围，以确保路径规划时，机器人的动作不会过于接近代价地图的边界，从而避免潜在的安全问题。

int i = 0;//i用于存储最近点的索引,初始化为0
    double sq_dist_threshold = dist_threshold * dist_threshold;//sq_dist_threshold是距离阈值的平方，用于比较距离
    double sq_dist = 1e10;//sq_dist初始化为一个非常大的数（1e10），用于存储当前最小距离的平方
    
    //we need to loop to a point on the plan that is within a certain distance of the robot
    bool robot_reached = false;//用于标记是否找到局部最小值
    //遍历全局路径的所有点，计算这些点与当前位置距离的平方，找到全局路径中与机器人最近的位置点
    for(int j=0; j < (int)global_plan.size(); ++j)
    {
      double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x;
      double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y;
      double new_sq_dist = x_diff * x_diff + y_diff * y_diff;
      //若车到达目的地并且是找不到离机器人更近的点了则break
      if (robot_reached && new_sq_dist > sq_dist)
        break;

      if (new_sq_dist < sq_dist) // find closest distance
      {
        sq_dist = new_sq_dist;
        i = j;
        if (sq_dist < 0.05)      // 2.5 cm to the robot; take the immediate local minima; if it's not the global
          robot_reached = true;  // minima, probably means that there's a loop in the path, and so we prefer this
      }
    }
    
    geometry_msgs::PoseStamped newer_pose;
    
    double plan_length = 0; // 用于累积路径长度
    
    //now we'll transform until points are outside of our distance threshold
    //将全局路径规划（global_plan）中的点转换到全局坐标系下，并且只保留那些在距离阈值内的点。
    //若设置了最大路径长度（max_plan_length），则只保留在最大路径长度内的点
    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];
      tf2::doTransform(pose, newer_pose, plan_to_global_transform);

      transformed_plan.push_back(newer_pose);

      double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
      double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;
      
      // 距离累加赋给plan_length
      if (i>0 && max_plan_length>0)
        plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);

      ++i;
    }

未完待续。。。