不知道这一篇还有没有人看…

这两个文件构成了算法的核心, 上一篇文章讲到在 eband_local_planner_ros.cpp 里, EBandPlannerROS::setplan 将全局路径通过之前 conversions_and_types.cpp 讲过的函数 “截断” 到局部, 之后交给 EBandPlanner eband_ 来操作 .

大家有兴趣可以改改代码, 打印打印信息看一下, 上面截断的全局路径在代码中反映的是一个数组, 数组每个元素由点的位置和姿态组成, 点的位置自然就是路径中的每个离散点在 map 下的坐标, 点的姿态只有终点被赋值, 中间的所有点的初始姿态都被赋值为0, 事实上, eband_->optimizeBand() 里会计算优化后的姿态数据(在2D 导航里也就是朝向)

下面我们直接来看 eband_->setplan() 和 eband_->optimizeBand()

eband_->setplan()

setplan 将截断的全局路径赋值给内置变量, 并优化路径里离散点的个数(全局路径点太密集了)

// 函数传入参数: 已经被截断的全局路径,即局部路径, 还未被优化
bool EBandPlanner::setPlan(
    const std::vector<geometry_msgs::PoseStamped> &global_plan) {
  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  // check if plan valid (minimum 2 frames)
  if (global_plan.size() < 2) {
    ROS_ERROR("Attempt to pass empty path to optimization. Valid path needs to "
              "have at least 2 Frames. This one has %d.",
              ((int)global_plan.size()));
    return false;
  }
  
  // copy plan to local member variable
  // 赋值给内部的成员变量, 下一步送去 optimizeBand() 优化
  global_plan_ = global_plan;

  // check whether plan and costmap are in the same frame
  if (global_plan.front().header.frame_id != costmap_ros_->getGlobalFrameID()) {
    ROS_ERROR("Elastic Band expects plan for optimization in the %s frame, the "
              "plan was sent in the %s frame.",
              costmap_ros_->getGlobalFrameID().c_str(),
              global_plan.front().header.frame_id.c_str());
    return false;
  }

  // convert frames in path into bubbles in band -> sets center of bubbles and
  // calculates expansion
  ROS_DEBUG("Converting Plan to Band");

  // 截至到这里,把传入的直接从 global 全局路径粗暴局部截断的路径转化成泡泡路径
  if (!convertPlanToBand(global_plan_, elastic_band_)) {
    ROS_WARN("Conversion from plan to elastic band failed. Plan probably not "
             "collision free. Plan not set for optimization");
    // TODO try to do local repairs of band
    return false;
  }

  // close gaps and remove redundant bubbles
  ROS_DEBUG("Refining Band");
	
  // 如果路径中点太密集, 将会去除掉冗余点, 如果点太稀疏, 会插值补充
  if (!refineBand(elastic_band_)) {
    ROS_WARN("Band is broken. Could not close gaps in converted path. Path not "
             "set. Global replanning needed");
    return false;
  }

  ROS_DEBUG("Refinement done - Band set.");
  return true;
}

setplan 函数最后调用了 refineBand 函数来优化路径中间点个数, 而 refineBand 简单的调用了 removeAndFill

refineBand()

bool EBandPlanner::refineBand(std::vector<Bubble> &band) {
  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  // check if band valid (minimum 2 bubbles)
  if (band.size() < 2) {
    ROS_WARN("Attempt to convert empty band to plan. Valid band needs to have "
             "at least 2 Frames. This one has %d.",
             ((int)band.size()));
    return false;
  }

  // instantiate local variables
  bool success;
  std::vector<Bubble> tmp_band;
  std::vector<Bubble>::iterator start_iter, end_iter;

  // remove redundant Bubbles and fill gabs recursively
  tmp_band = band;
  start_iter = tmp_band.begin();
  end_iter = (tmp_band.end() - 1); // -1 because .end() points "past the end"!

  success = removeAndFill(tmp_band, start_iter, end_iter);

  if (!success)
    ROS_DEBUG("Band is broken. Could not close gaps.");
  else {
#ifdef DEBUG_EBAND_
    ROS_DEBUG("Recursive filling and removing DONE");
#endif
    band = tmp_band;
  }

  return success;
}

removeAndFill()

来看看 removeAndFill 在干嘛, 大家可以看看源码里用于 debug 模式的一些调试代码, 里面其实展示了代码中每个步骤的很多信息.

这个函数主要用二分法来处理泡泡路径带, 发现每两个点之间距离太近就移除, 发现隔得太远就插值.

bool EBandPlanner::removeAndFill(std::vector<Bubble> &band,
                                 std::vector<Bubble>::iterator &start_iter,
                                 std::vector<Bubble>::iterator &end_iter) {
  // instantiate local variables
  bool overlap;
  std::vector<Bubble>::iterator tmp_iter;
  int mid_int, diff_int;

#ifdef DEBUG_EBAND_
  int debug_dist_start, debug_dist_iters;
  debug_dist_start = std::distance(band.begin(), start_iter);
  debug_dist_iters = std::distance(start_iter, end_iter);
  ROS_DEBUG("Refining Recursive - Check if Bubbles %d and %d overlapp. Total "
            "size of band %d.",
            debug_dist_start, (debug_dist_start + debug_dist_iters),
            ((int)band.size()));
#endif

  // check that iterators are still valid
  ROS_ASSERT(start_iter >= band.begin());
  ROS_ASSERT(
      end_iter <
      band.end()); // "<" because .end() points _behind_ last element of vector
  ROS_ASSERT(start_iter < end_iter);

  // check whether start and end bubbles of this intervall overlap
  overlap = checkOverlap(*start_iter, *end_iter);

  if (overlap) {

#ifdef DEBUG_EBAND_
    ROS_DEBUG("Refining Recursive - Bubbles overlapp, check for redundancies");
#endif

    // if there are bubbles between start and end of intervall remove them (they
    // are redundant as start and end of intervall do overlap)
    if ((start_iter + 1) < end_iter) {
#ifdef DEBUG_EBAND_
      ROS_DEBUG(
          "Refining Recursive - Bubbles overlapp, removing Bubbles %d to %d.",
          (debug_dist_start + 1), (debug_dist_start + debug_dist_iters - 1));
#endif

      // erase bubbles between start and end (but not start and end themself)
      // and get new iterator to end (old one is invalid)
      tmp_iter = band.erase((start_iter + 1), end_iter);

      // write back changed iterator pointing to the end of the intervall
      end_iter = tmp_iter;
    }

#ifdef DEBUG_EBAND_
    ROS_DEBUG("Refining Recursive - Bubbles overlapp - DONE");
#endif

    // we are done here (leaf of this branch is reached)
    return true;
  }

  // if bubbles do not overlap -> check whether there are still bubbles between
  // start and end
  if ((start_iter + 1) < end_iter) {
#ifdef DEBUG_EBAND_
    ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion "
              "deeper");
#endif

    // split remaining sequence of bubbles
    // get distance between start and end iterator for this intervall
    mid_int = std::distance(start_iter, end_iter);
    mid_int = mid_int / 2; // division by integer implies floor (round down)

    // now get iterator pointing to the middle (roughly)
    tmp_iter = start_iter + mid_int;
    // and realative position of end_iter to tmp_iter
    diff_int = (int)std::distance(tmp_iter, end_iter);

    // after all this arithmetics - check that iterators are still valid
    ROS_ASSERT(start_iter >= band.begin());
    ROS_ASSERT(end_iter < band.end()); // "<" because .end() points _behind_
                                       // last element of vector
    ROS_ASSERT(start_iter < end_iter);

    // and call removeAndFill recursively for the left intervall
    if (!removeAndFill(band, start_iter, tmp_iter)) {
      // band is broken in this intervall and could not be fixed
      return false;
    }

    // carefull at this point!!! if we filled in or removed bubbles end_iter is
    // not valid anymore but the relative position towards tmp_iter is still the
    // same and tmp_iter was kept valid in the lower recursion steps
    end_iter = tmp_iter + diff_int;

    // check that iterators are still valid - one more time
    ROS_ASSERT(start_iter >= band.begin());
    ROS_ASSERT(end_iter < band.end()); // "<" because .end() points _behind_
                                       // last element of vector
    ROS_ASSERT((start_iter < tmp_iter) && (tmp_iter < end_iter));

    // o.k. we are done with left hand intervall now do the same for the right
    // hand intervall but first get relative position of start and tmp iter
    diff_int = (int)std::distance(start_iter, tmp_iter);
    if (!removeAndFill(band, tmp_iter, end_iter)) {
      // band is broken in this intervall and could not be fixed
      return false;
    }

    // if we filled in bubbles vector might have been reallocated -> start_iter
    // might be invalid
    start_iter = tmp_iter - diff_int;

    // check that iterators are still valid - almost done
    ROS_ASSERT(start_iter >= band.begin());
    ROS_ASSERT(end_iter < band.end()); // "<" because .end() points _behind_
                                       // last element of vector
    ROS_ASSERT((start_iter < tmp_iter) && (tmp_iter < end_iter));

    // we reached the leaf but we are not yet done
    // -> we know that there are no redundant elements in the left intervall
    // taken on its own
    // -> and we know the same holds for the right intervall
    // but the middle bubble itself might be redundant -> check it
    if (checkOverlap(*(tmp_iter - 1), *(tmp_iter + 1))) {
#ifdef DEBUG_EBAND_
      ROS_DEBUG("Refining Recursive - Removing middle bubble");
#endif

      // again: get distance between (tmp_iter + 1) and end_iter, (+1 because we
      // will erase tmp_iter itself)
      diff_int = (int)std::distance((tmp_iter + 1), end_iter);

      // remove middle bubble and correct end_iter
      tmp_iter = band.erase(tmp_iter);
      end_iter = tmp_iter + diff_int;
    }

    // check that iterators are still valid - almost almost
    ROS_ASSERT(start_iter >= band.begin());
    ROS_ASSERT(end_iter < band.end()); // "<" because .end() points _behind_
                                       // last element of vector
    ROS_ASSERT(start_iter < end_iter);

#ifdef DEBUG_EBAND_
    ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion "
              "deeper DONE");
#endif

    // now we are done with this case
    return true;
  }

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Refining Recursive - Gap detected, fill recursive");
#endif

  // last possible case -> bubbles do not overlap AND there are nor bubbles in
  // between -> try to fill gap recursively
  if (!fillGap(band, start_iter, end_iter)) {
    // band is broken in this intervall and could not be fixed (this should only
    // be called on a leaf, so we put a log_out here;)
    ROS_DEBUG("Failed to fill gap between bubble %d and %d.",
              (int)distance(band.begin(), start_iter),
              (int)distance(band.begin(), end_iter));
    return false;
  }

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Refining Recursive - Gap detected, fill recursive DONE");
#endif

  // we could fill the gap (reached leaf of this branch)
  return true;
}

interpolateBubbles()

插值函数是 interpolateBubbles, 这个函数先调用 fillGap , 再在 fillGap 里调用 interpolateBubbles, interpolateBubbles 直接使用中值插值, 位置和姿态一起插

bool EBandPlanner::interpolateBubbles(
    geometry_msgs::PoseStamped start_center,
    geometry_msgs::PoseStamped end_center,
    geometry_msgs::PoseStamped &interpolated_center) {
  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  // instantiate local variables
  geometry_msgs::Pose2D start_pose2D, end_pose2D, interpolated_pose2D;
  double delta_theta;

  // copy header
  interpolated_center.header = start_center.header;

  // interpolate angles
  // TODO make this in a better way
  // - for instance use "slerp" to interpolate directly between quaternions
  // - or work with pose2D right from the beginnning
  // convert quaternions to euler angles - at this point this no longer works in
  // 3D !!
  PoseToPose2D(start_center.pose, start_pose2D);
  PoseToPose2D(end_center.pose, end_pose2D);
  // calc mean of theta angle
  delta_theta = end_pose2D.theta - start_pose2D.theta;
  delta_theta = angles::normalize_angle(delta_theta) / 2.0;
  interpolated_pose2D.theta = start_pose2D.theta + delta_theta;
  interpolated_pose2D.theta =
      angles::normalize_angle(interpolated_pose2D.theta);
  // convert back to quaternion
  interpolated_pose2D.x = 0.0;
  interpolated_pose2D.y = 0.0;
  Pose2DToPose(interpolated_center.pose, interpolated_pose2D);

  // interpolate positions
  interpolated_center.pose.position.x =
      (end_center.pose.position.x + start_center.pose.position.x) / 2.0;
  interpolated_center.pose.position.y =
      (end_center.pose.position.y + start_center.pose.position.y) / 2.0;
  interpolated_center.pose.position.z =
      (end_center.pose.position.z + start_center.pose.position.z) / 2.0;

  // TODO ideally this would take into account kinematics of the robot and for
  // instance use splines

  return true;
}

eband_->optimizeBand()

这个函数实现论文算法, 输入截断的全局路径,输出优化后的路径, 注意这个函数是个重载函数, 在无参数版本里调用有参数版本

无参数版本 :

bool EBandPlanner::optimizeBand() {
  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  // check if there is a band
  if (elastic_band_.empty()) {
    ROS_ERROR("Band is empty. Probably Band has not been set yet");
    return false;
  }

  // call optimization with member elastic_band_
  ROS_DEBUG("Starting optimization of band");

  // 调用有参数版本
  if (!optimizeBand(elastic_band_)) {
    ROS_DEBUG("Aborting Optimization. Changes discarded.");
    return false;
  }

  ROS_DEBUG("Elastic Band - Optimization successfull!");
  return true;
}

有参数版本 :

有参数版本依次调用了如下几个函数:

• calcObstacleKinematicDistance(): 通过 costmap_2d 初始化泡泡的半径
• refineBand(): 在算法优化之前, 先对整条泡泡路径进行一次小优化, 主要包括去除冗余泡泡和泡泡插值
• modifyBandArtificialForce(): 论文算法实现部分, 生成算法优化后的泡泡路径
• refineBand(): 在算法优化之后, 再对整条泡泡路径进行一次小优化

bool EBandPlanner::optimizeBand(std::vector<Bubble> &band) {
  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  // check whether band and costmap are in the same frame
  if (band.front().center.header.frame_id != costmap_ros_->getGlobalFrameID()) {
    ROS_ERROR("Elastic Band expects plan for optimization in the %s frame, the "
              "plan was sent in the %s frame.",
              costmap_ros_->getGlobalFrameID().c_str(),
              band.front().center.header.frame_id.c_str());
    return false;
  }

  double distance;
  for (int i = 0; i < ((int)band.size()); i++) {
    // update Size of Bubbles in band by calculating Dist to nearest Obstacle
    // [depends kinematic, environment]
    if (!calcObstacleKinematicDistance(band.at(i).center.pose, distance)) {
      ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. "
                "Frame %d of %d Probably outside map coordinates.",
                i, ((int)band.size()));
      return false;
    }

    if (distance == 0.0) {
      // frame must not be immediately in collision -> otherwise calculation of
      // gradient will later be invalid
      ROS_DEBUG(
          "Optimization (Elastic Band) - Calculation of Distance failed. Frame "
          "%d of %d in collision. Plan invalid. Trying to refine band.",
          i, ((int)band.size()));
      // TODO if frame in collision try to repair band instead of aborting
      // everything
      return false;
    }

    band.at(i).expansion = distance;
  }

  // close gaps and remove redundant bubbles
  if (!refineBand(band)) {
    ROS_DEBUG("Elastic Band is broken. Could not close gaps in band. Global "
              "replanning needed.");
    return false;
  }

  // get a copy of current (valid) band
  std::vector<Bubble> tmp_band = band;

  // now optimize iteratively (for instance by miminizing the energy-function of
  // the full system)
  for (int i = 0; i < num_optim_iterations_; i++) {
    ROS_DEBUG("Inside optimization: Cycle no %d", i);

    // ROS_INFO("33[1;32mInside optimization: Cycle no %d33[0m", i);

    // calculate forces and apply changes
    if (!modifyBandArtificialForce(tmp_band)) {
      ROS_DEBUG("Optimization failed while trying to modify Band.");
      // something went wrong -> discard changes and stop process
      return false;
    }

    // check whether band still valid - refine if neccesarry
    if (!refineBand(tmp_band)) {
      ROS_DEBUG("Optimization failed while trying to refine modified band");
      // modified band is not valid anymore -> discard changes and stop process
      return false;
    }
    
  // copy changes back to band
  band = tmp_band;
  return true;
}

calcObstacleKinematicDistance()

这个函数很简单, 拿costmap 的占有值去除以一个参数因子 weight 获得距离, 这个参数在 yaml 文件里叫 costmap_weight, 怎么调参就靠手感了

bool EBandPlanner::calcObstacleKinematicDistance(
    geometry_msgs::Pose center_pose, double &distance) {
  // calculate distance to nearest obstacle [depends kinematic, shape,
  // environment]

  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  unsigned int cell_x, cell_y;
  unsigned char disc_cost;
  double weight = costmap_weight_;

  // read distance to nearest obstacle directly from costmap
  // (does not take into account shape and kinematic properties)
  // get cell for coordinates of bubble center
  if (!costmap_->worldToMap(center_pose.position.x, center_pose.position.y,
                            cell_x, cell_y)) {
    // probably at the edge of the costmap - this value should be recovered soon
    disc_cost = 1;
  } else {
    // get cost for this cell
    disc_cost = costmap_->getCost(cell_x, cell_y);
  }

  // calculate distance to nearest obstacel from this cost (see costmap_2d in
  // wiki for details)

  // For reference: here comes an excerpt of the cost calculation within the
  // costmap function
  /*if(distance == 0)
    cost = LETHAL_OBSTACLE;
    else if(distance <= cell_inscribed_radius_)
    cost = INSCRIBED_INFLATED_OBSTACLE;
    else {
  //make sure cost falls off by Euclidean distance
  double euclidean_distance = distance * resolution_;
  double factor = exp(-1.0 * weight_ * (euclidean_distance -
  inscribed_radius_)); cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1)
  * factor);
  }*/

  if (disc_cost == costmap_2d::LETHAL_OBSTACLE) { // 254
    // pose is inside an obstacle - very bad
    distance = 0.0;
  } else if (disc_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { // 253
    // footprint is definitely inside an obstacle - still bad
    distance = 0.0;
  } else {
    if (disc_cost == 0) {          // freespace, no estimate of distance
      disc_cost = 1;               // lowest non freespace cost
    } else if (disc_cost == 255) { // unknown space, we should never be here
      disc_cost = 1;
    }
    // factor 算出来一般是 1 / 253-1 = 0.004
    double factor =
        ((double)disc_cost) / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1);
    // ln(0.004) = -5.521
    distance = -log(factor) / weight;
    // ROS_INFO("factor: %.3f", factor);
  }
  // ROS_INFO("disc_cost: %d distance: %.3f", disc_cost, distance);

  return true;
}

modifyBandArtificialForce()

先复习一下论文, 论文中为了维持弹力带平衡, 需要计算弹力带中每个泡泡的内力和外力(斥力), 并消除切向力影响, 代码流程与论文一致, 依次调用计算内力、计算斥力及计算排斥力的函数, 算完之后把力作用到泡泡上，即修改路径

bool EBandPlanner::modifyBandArtificialForce(std::vector<Bubble> &band) {
  ROS_INFO("33[1;32m In modifyBandArtificialForce33[0m");

  if (band.empty()) {
    ROS_ERROR("Trying to modify an empty band.");
    return false;
  }

  if (band.size() <= 2) {
    // nothing to do here -> we can stop right away
    return true;
  }

  std::vector<geometry_msgs::WrenchStamped> internal_forces, external_forces,
      forces;
  geometry_msgs::WrenchStamped wrench;

#ifdef DEBUG_EBAND_
  // publish original band
  if (visualization_)
    eband_visual_->publishBand("bubbles", band);
#endif

  // init variables to calm down debug warnings
  // 这是一个力螺旋，　有力和力矩
  wrench.header.stamp = ros::Time::now();
  wrench.header.frame_id = band[0].center.header.frame_id;
  wrench.wrench.force.x = 0.0;
  wrench.wrench.force.y = 0.0;
  wrench.wrench.force.z = 0.0;
  wrench.wrench.torque.x = 0.0;
  wrench.wrench.torque.y = 0.0;
  wrench.wrench.torque.z = 0.0;
  internal_forces.assign(band.size(), wrench);
  external_forces = internal_forces;
  forces = internal_forces;

  // TODO log timigs of planner
  // instantiate variables for timing
  // ros::Time time_stamp1, time_stamp2;
  // ros::Duration duration;
  // time_stamp1 = ros::Time::now();

  // due to refinement band might change its size -> use while loop
  int i = 1;
  bool forward = true; // cycle 1xforwards and 1xbackwards through band

  // 循环,对 band 路径带里的每个泡泡做操作
  // 这个 while 循环条件切去了 band 头尾,即切去了当前点和目标点
  // 这个循环先从前往后,到了后面有个条件再从后往前
  // 即 当前点->目标点->当前点 为一次优化循环
  ROS_INFO("bubble band.size(): %d.", band.size());

  while ((i > 0) && (i < ((int)band.size() - 1))) {
    ROS_DEBUG("Modifying bubble %d.", i);

#ifdef DEBUG_EBAND_
    ROS_DEBUG("Calculating internal force for bubble %d.", i);
#endif

    // 计算内力
    if (!calcInternalForces(i, band, band.at(i), internal_forces.at(i))) {
      // calculation of internal forces failed - stopping optimization
      ROS_DEBUG("Calculation of internal forces failed");
      return false;
    }

#ifdef DEBUG_EBAND_
    if (visualization_)
      // publish internal forces
      eband_visual_->publishForce("internal_forces", i, eband_visual_->blue,
                                  internal_forces[i], band[i]);
    // Log out debug info about next step
    ROS_DEBUG("Calculating external force for bubble %d.", i);
#endif

    // 计算斥力
    // if(!calcExternalForces(i, band, external_forces))
    if (!calcExternalForces(i, band.at(i), external_forces.at(i))) {
      // calculation of External Forces failed - stopping optimization
      ROS_DEBUG("Calculation of external forces failed");
      return false;
    }

#ifdef DEBUG_EBAND_
    if (visualization_)
      // publish external forces
      eband_visual_->publishForce("external_forces", i, eband_visual_->red,
                                  external_forces[i], band[i]);
    // Log out debug info about next step
    ROS_DEBUG("Superposing internal and external forces");
#endif

    // sum up external and internal forces over all bubbles
    forces.at(i).wrench.force.x = internal_forces.at(i).wrench.force.x +
                                  external_forces.at(i).wrench.force.x;
    forces.at(i).wrench.force.y = internal_forces.at(i).wrench.force.y +
                                  external_forces.at(i).wrench.force.y;
    forces.at(i).wrench.force.z = internal_forces.at(i).wrench.force.z +
                                  external_forces.at(i).wrench.force.z;

    forces.at(i).wrench.torque.x = internal_forces.at(i).wrench.torque.x +
                                   external_forces.at(i).wrench.torque.x;
    forces.at(i).wrench.torque.y = internal_forces.at(i).wrench.torque.y +
                                   external_forces.at(i).wrench.torque.y;
    forces.at(i).wrench.torque.z = internal_forces.at(i).wrench.torque.z +
                                   external_forces.at(i).wrench.torque.z;

#ifdef DEBUG_EBAND_
    ROS_DEBUG("Superpose forces: (x, y, theta) = (%f, %f, %f)",
              forces.at(i).wrench.force.x, forces.at(i).wrench.force.y,
              forces.at(i).wrench.torque.z);
    ROS_DEBUG("Supressing tangential forces");
#endif

    // 计算切向力
    if (!suppressTangentialForces(i, band, forces.at(i))) {
      // suppression of tangential forces failed
      ROS_DEBUG("Supression of tangential forces failed");
      return false;
    }

#ifdef DEBUG_EBAND_
    if (visualization_)
      // publish resulting forces
      eband_visual_->publishForce("resulting_forces", i, eband_visual_->green,
                                  forces[i], band[i]);
#endif

    // if (visualization_)
    //   // publish resulting forces
    //   eband_visual_->publishForce("resulting_forces", i, eband_visual_->red,
    //                               forces[i], band[i]);

    ROS_DEBUG("Applying forces to modify band");
    // 把力作用于泡泡
    if (!applyForces(i, band, forces)) {
      // band invalid
      ROS_DEBUG("Band is invalid - Stopping Modification");
      return false;
    }

#ifdef DEBUG_EBAND_
    if (visualization_) {
      // publish band with changed bubble at resulting position
      eband_visual_->publishBand("bubbles", band);
      ros::Duration(0.01).sleep();
    }
#endif

    // next bubble
    if (forward) {
      i++;
      if (i == ((int)band.size() - 1)) {
        // reached end of band - start backwards cycle until at start again -
        // then stop
        forward = false;
        i--;
        ROS_DEBUG("Optimization Elastic Band - Forward cycle done, starting "
                  "backward cycle");
      }
    } else {
      i--;
    }
  }

  return true;
}

calcInternalForces()　计算内力

论文中的内力计算公式

其中，$k_c$为全局收缩增益(contraction gain)，是个参数，这个内力与前一个泡泡和下一个泡泡到当前泡泡的位姿差有关。

代码和这个一模一样。

bool EBandPlanner::calcInternalForces(int bubble_num, std::vector<Bubble> band,
                                      Bubble curr_bubble,
                                      geometry_msgs::WrenchStamped &forces) {
  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  // cycle over all bubbles except first and last (these are fixed)
  if (band.size() <= 2) {
    // nothing to do here -> we can stop right away - no forces calculated
    return true;
  }

  // init tmp variables
  double distance1, distance2;
  geometry_msgs::Twist difference1, difference2;
  geometry_msgs::Wrench wrench;

  // make sure this method was called for a valid element in the forces or
  // bubbles vector
  ROS_ASSERT(bubble_num > 0);
  ROS_ASSERT(bubble_num < ((int)band.size() - 1));

  // get distance between bubbles
  if (!calcBubbleDistance(curr_bubble.center.pose,
                          band[bubble_num - 1].center.pose, distance1)) {
    ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting "
              "calculation of internal forces!");
    return false;
  }

  if (!calcBubbleDistance(curr_bubble.center.pose,
                          band[bubble_num + 1].center.pose, distance2)) {
    ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting "
              "calculation of internal forces!");
    return false;
  }

  // get (elementwise) difference bewtween bubbles
  if (!calcBubbleDifference(curr_bubble.center.pose,
                            band[bubble_num - 1].center.pose, difference1)) {
    ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting "
              "calculation of internal forces!");
    return false;
  }

  if (!calcBubbleDifference(curr_bubble.center.pose,
                            band[bubble_num + 1].center.pose, difference2)) {
    ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting "
              "calculation of internal forces!");
    return false;
  }

  // make sure to avoid division by  (almost) zero during force calculation
  // (avoid numerical problems)
  // -> if difference/distance is (close to) zero then the force in this
  // direction should be zero as well
  if (distance1 <= tiny_bubble_distance_)
    distance1 = 1000000.0;
  if (distance2 <= tiny_bubble_distance_)
    distance2 = 1000000.0;

  // now calculate wrench - forces model an elastic band and are normed
  // (distance) to render forces for small and large bubbles the same
  // 与论文一致
  // 力的 x y z 分量也即按照公式，计算　增益 x (与上一个泡泡的单位距离 + 与下一个泡泡的单位距离)
  wrench.force.x = internal_force_gain_ * (difference1.linear.x / distance1 +
                                           difference2.linear.x / distance2);
  wrench.force.y = internal_force_gain_ * (difference1.linear.y / distance1 +
                                           difference2.linear.y / distance2);
  wrench.force.z = internal_force_gain_ * (difference1.linear.z / distance1 +
                                           difference2.linear.z / distance2);
  wrench.torque.x = internal_force_gain_ * (difference1.angular.x / distance1 +
                                            difference2.angular.x / distance2);
  wrench.torque.y = internal_force_gain_ * (difference1.angular.y / distance1 +
                                            difference2.angular.y / distance2);
  wrench.torque.z = internal_force_gain_ * (difference1.angular.z / distance1 +
                                            difference2.angular.z / distance2);

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Calculating internal forces: (x, y, theta) = (%f, %f, %f)",
            wrench.force.x, wrench.force.y, wrench.torque.z);
#endif

  // store wrench in vector
  forces.wrench = wrench;

  return true;
}

calcExternalForces()　计算外力

复习论文：

外力由下式决定

其中，$k_c$为全局斥力增益(repulsion gain)，是个参数

$$\frac{\partial \rho }{\partial b}$$ 为泡泡半径关于泡泡中心位姿 b 的导数，由下式决定

其中， $h$ 为步长，设置为 $\rho (b)$， 即当前位姿泡泡半径。

论文这样实现：

bool EBandPlanner::calcExternalForces(int bubble_num, Bubble curr_bubble,
                                      geometry_msgs::WrenchStamped &forces) {
  // check if plugin initialized
  if (!initialized_) {
    ROS_ERROR("This planner has not been initialized, please call initialize() "
              "before using this planner");
    return false;
  }

  // init tmp variables
  double distance1, distance2;
  geometry_msgs::Pose edge;
  geometry_msgs::Pose2D edge_pose2D;
  geometry_msgs::Wrench wrench;

  // calculate delta-poses (on upper edge of bubble) for x-direction
  edge = curr_bubble.center.pose;
  
  // 中心往前走一个半径的距离，可以想像，泡泡是个圆，假设 x 方向指向上，
  // 则中心往前走一个半径的距离到了泡泡的上边缘
  // distance1 为泡泡的一个边缘到障碍物的距离
  edge.position.x = edge.position.x + curr_bubble.expansion;
  
  // get expansion on bubble at this point
  if (!calcObstacleKinematicDistance(edge, distance1)) {
    ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance "
              "information to calculate external forces",
              bubble_num);
    // we cannot calculate external forces for this bubble - but still continue
    // for the other bubbles
    return true;
  }
  // calculate delta-poses (on lower edge of bubble) for x-direction
  // 到了泡泡的下边缘
  // distance２ 为泡泡的另一个边缘到障碍物的距离
  edge.position.x = edge.position.x - 2.0 * curr_bubble.expansion;
 
  // get expansion on bubble at this point
  if (!calcObstacleKinematicDistance(edge, distance2)) {
    ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance "
              "information to calculate external forces",
              bubble_num);
    // we cannot calculate external forces for this bubble - but still continue
    // for the other bubbles
    return true;
  }

  // calculate difference-quotient (approx. of derivative) in x-direction
  if (curr_bubble.expansion <= tiny_bubble_expansion_) {
    // avoid division by (almost) zero to avoid numerical problems
    wrench.force.x = -external_force_gain_ * (distance2 - distance1) /
                     (2.0 * tiny_bubble_expansion_);
    // actually we should never end up here - band should have been considered
    // as broken
    ROS_DEBUG("Calculating external forces on broken band. Bubble should have "
              "been removed. Local Planner probably ill configured");
  } else
    wrench.force.x = -external_force_gain_ * (distance2 - distance1) /
                     (2.0 * curr_bubble.expansion);
  // TODO above equations skip term to make forces continuous at end of
  // influence region - test to add corresponding term

  // calculate delta-poses (on upper edge of bubble) for y-direction
  edge = curr_bubble.center.pose;
  edge.position.y = edge.position.y + curr_bubble.expansion;
  // get expansion on bubble at this point
  if (!calcObstacleKinematicDistance(edge, distance1)) {
    ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance "
              "information to calculate external forces",
              bubble_num);
    // we cannot calculate external forces for this bubble - but still continue
    // for the other bubbles
    return true;
  }
  // calculate delta-poses (on lower edge of bubble) for x-direction
  edge.position.y = edge.position.y - 2.0 * curr_bubble.expansion;
  // get expansion on bubble at this point
  if (!calcObstacleKinematicDistance(edge, distance2)) {
    ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance "
              "information to calculate external forces",
              bubble_num);
    // we cannot calculate external forces for this bubble - but still continue
    // for the other bubbles
    return true;
  }

  // calculate difference-quotient (approx. of derivative) in x-direction
  if (curr_bubble.expansion <= tiny_bubble_expansion_) {
    // avoid division by (almost) zero to avoid numerical problems
    wrench.force.y = -external_force_gain_ * (distance2 - distance1) /
                     (2.0 * tiny_bubble_expansion_);
    // actually we should never end up here - band should have been considered
    // as broken
    ROS_DEBUG("Calculating external forces on broken band. Bubble should have "
              "been removed. Local Planner probably ill configured");
  } else
    wrench.force.y = -external_force_gain_ * (distance2 - distance1) /
                     (2.0 * curr_bubble.expansion);
  // TODO above equations skip term to make forces continuous at end of
  // influence region - test to add corresponsing term

  // no force in z-direction
  wrench.force.z = 0.0;

  // no torque around x and y axis
  wrench.torque.x = 0.0;
  wrench.torque.y = 0.0;

  // calculate delta-poses (on upper edge of bubble) for x-direction
  PoseToPose2D(curr_bubble.center.pose, edge_pose2D);
  edge_pose2D.theta =
      edge_pose2D.theta +
      (curr_bubble.expansion / getCircumscribedRadius(*costmap_ros_));
  edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta);
  PoseToPose2D(edge, edge_pose2D);
  // get expansion on bubble at this point
  if (!calcObstacleKinematicDistance(edge, distance1)) {
    ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance "
              "information to calculate external forces",
              bubble_num);
    // we cannot calculate external forces for this bubble - but still continue
    // for the other bubbles
    return true;
  }
  // calculate delta-poses (on lower edge of bubble) for x-direction
  edge_pose2D.theta =
      edge_pose2D.theta -
      2.0 * (curr_bubble.expansion / getCircumscribedRadius(*costmap_ros_));
  edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta);
  PoseToPose2D(edge, edge_pose2D);
  // get expansion on bubble at this point
  if (!calcObstacleKinematicDistance(edge, distance2)) {
    ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance "
              "information to calculate external forces",
              bubble_num);
    // we cannot calculate external forces for this bubble - but still continue
    // for the other bubbles
    return true;
  }

  // calculate difference-quotient (approx. of derivative) in x-direction
  if (curr_bubble.expansion <= tiny_bubble_expansion_) {
    // avoid division by (almost) zero to avoid numerical problems
    wrench.torque.z = -external_force_gain_ * (distance2 - distance1) /
                      (2.0 * tiny_bubble_expansion_);
    // actually we should never end up here - band should have been considered
    // as broken
    ROS_DEBUG("Calculating external forces on broken band. Bubble should have "
              "been removed. Local Planner probably ill configured");
  } else
    wrench.torque.z = -external_force_gain_ * (distance2 - distance1) /
                      (2.0 * curr_bubble.expansion);
    // TODO above equations skip term to make forces continuous at end of
    // influence region - test to add corresponsing term

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Calculating external forces: (x, y, theta) = (%f, %f, %f)",
            wrench.force.x, wrench.force.y, wrench.torque.z);
#endif

  // assign wrench to forces vector
  forces.wrench = wrench;

  return true;
}

suppressTangentialForces() 移除切向力

和论文一样

bool EBandPlanner::suppressTangentialForces(
    int bubble_num, std::vector<Bubble> band,
    geometry_msgs::WrenchStamped &forces) {
  // cycle over all bubbles except first and last (these are fixed)
  if (band.size() <= 2) {
    // nothing to do here -> we can stop right away - no forces calculated
    return true;
  }

  double scalar_fd, scalar_dd;
  geometry_msgs::Twist difference;

  // make sure this method was called for a valid element in the forces or
  // bubbles vector
  ROS_ASSERT(bubble_num > 0);
  ROS_ASSERT(bubble_num < ((int)band.size() - 1));

  // get pose-difference from following to preceding bubble -> "direction of the
  // band in this bubble"
  if (!calcBubbleDifference(band[bubble_num + 1].center.pose,
                            band[bubble_num - 1].center.pose, difference))
    return false;

  // "project wrench" in middle bubble onto connecting vector
  // scalar wrench*difference
  scalar_fd = forces.wrench.force.x * difference.linear.x +
              forces.wrench.force.y * difference.linear.y +
              forces.wrench.force.z * difference.linear.z +
              forces.wrench.torque.x * difference.angular.x +
              forces.wrench.torque.y * difference.angular.y +
              forces.wrench.torque.z * difference.angular.z;

  // abs of difference-vector: scalar difference*difference
  scalar_dd = difference.linear.x * difference.linear.x +
              difference.linear.y * difference.linear.y +
              difference.linear.z * difference.linear.z +
              difference.angular.x * difference.angular.x +
              difference.angular.y * difference.angular.y +
              difference.angular.z * difference.angular.z;

  // avoid division by (almost) zero -> check if bubbles have (almost) same
  // center-pose
  if (scalar_dd <= tiny_bubble_distance_) {
    // there are redundant bubbles, this should normally not hapen -> probably
    // error in band refinement
    ROS_DEBUG(
        "Calculating tangential forces for redundant bubbles. Bubble should "
        "have been removed. Local Planner probably ill configured");
  }

  // calculate orthogonal components
  forces.wrench.force.x =
      forces.wrench.force.x - scalar_fd / scalar_dd * difference.linear.x;
  forces.wrench.force.y =
      forces.wrench.force.y - scalar_fd / scalar_dd * difference.linear.y;
  forces.wrench.force.z =
      forces.wrench.force.z - scalar_fd / scalar_dd * difference.linear.z;
  forces.wrench.torque.x =
      forces.wrench.torque.x - scalar_fd / scalar_dd * difference.angular.x;
  forces.wrench.torque.y =
      forces.wrench.torque.y - scalar_fd / scalar_dd * difference.angular.y;
  forces.wrench.torque.z =
      forces.wrench.torque.z - scalar_fd / scalar_dd * difference.angular.z;

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Supressing tangential forces: (x, y, theta) = (%f, %f, %f)",
            forces.wrench.force.x, forces.wrench.force.y,
            forces.wrench.torque.z);
#endif

  return true;
}

applyForces()

计算完三个力之后，将计算出的力作用于泡泡上，优化弹力带

bool EBandPlanner::applyForces(
    int bubble_num, std::vector<Bubble> &band,
    std::vector<geometry_msgs::WrenchStamped> forces) {
  // cycle over all bubbles except first and last (these are fixed)
  if (band.size() <= 2) {
    // nothing to do here -> we can stop right away - no forces calculated
    return true;
  }

  geometry_msgs::Pose2D bubble_pose2D, new_bubble_pose2D;
  geometry_msgs::Pose bubble_pose, new_bubble_pose;
  geometry_msgs::Twist bubble_jump;
  Bubble new_bubble = band.at(bubble_num);
  double distance;

  // move bubble
  bubble_pose = band.at(bubble_num).center.pose;
  PoseToPose2D(bubble_pose, bubble_pose2D);

  // move according to bubble_new = bubble_old + alpha*force -> we choose alpha
  // to be the current expansion of the modified bubble
  //改变泡泡,往外推
  // 就是根据之前计算出的力，将未经优化的路径进行优化，优化的结果的改变位置的方向
  bubble_jump.linear.x =
      band.at(bubble_num).expansion * forces.at(bubble_num).wrench.force.x;
  bubble_jump.linear.y =
      band.at(bubble_num).expansion * forces.at(bubble_num).wrench.force.y;
  bubble_jump.linear.z = 0.0;
  bubble_jump.angular.x = 0.0;
  bubble_jump.angular.y = 0.0;

  // 扭矩对转向的作用
  bubble_jump.angular.z = band.at(bubble_num).expansion /
                          getCircumscribedRadius(*costmap_ros_) *
                          forces.at(bubble_num).wrench.torque.z;
  
  bubble_jump.angular.z = angles::normalize_angle(bubble_jump.angular.z);

  // apply changes to calc tmp bubble position
  new_bubble_pose2D.x = bubble_pose2D.x + bubble_jump.linear.x;
  new_bubble_pose2D.y = bubble_pose2D.y + bubble_jump.linear.y;
  new_bubble_pose2D.theta = bubble_pose2D.theta + bubble_jump.angular.z;
  new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta);

  // apply changes to local copy
  Pose2DToPose(new_bubble_pose, new_bubble_pose2D);
  new_bubble.center.pose = new_bubble_pose;

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Try moving bubble %d at (%f, %f, %f) by (%f, %f, %f).", bubble_num,
            bubble_pose2D.x, bubble_pose2D.y, bubble_pose2D.theta,
            bubble_jump.linear.x, bubble_jump.linear.y, bubble_jump.angular.z);
#endif

  // check validity of moved bubble

  // recalc expansion of bubble -> calc Size of Bubbles by calculating Dist to
  // nearest Obstacle [depends kinematic, environment]
  if (!calcObstacleKinematicDistance(new_bubble_pose, distance)) {
    ROS_DEBUG("Calculation of Distance failed. Frame %d of %d Probably outside "
              "map. Discarding Changes",
              bubble_num, ((int)band.size()));

#ifdef DEBUG_EBAND_
    if (visualization_)
      eband_visual_->publishBubble("bubble_hypo", bubble_num,
                                   eband_visual_->red, new_bubble);
#endif

    // this bubble must not be changed, but band is still valid -> continue with
    // other bubbles
    return true;
  }

  if (distance <= tiny_bubble_expansion_) {
    // frame must not be immediately in collision -> otherwise calculation of
    // gradient will later be invalid
    ROS_DEBUG("Calculation of Distance failed. Frame %d of %d in collision. "
              "Plan invalid. Discarding Changes",
              bubble_num, ((int)band.size()));

#ifdef DEBUG_EBAND_
    if (visualization_)
      eband_visual_->publishBubble("bubble_hypo", bubble_num,
                                   eband_visual_->red, new_bubble);
#endif

    // this bubble must not be changed, but band is still valid -> continue with
    // other bubbles
    return true;
  }

  // so far o.k. -> assign distance to new bubble
  new_bubble.expansion = distance;

  // check whether step was reasonable

  geometry_msgs::WrenchStamped new_bubble_force = forces.at(bubble_num);

  // check whether we get a valid force calculation here
  if (!getForcesAt(bubble_num, band, new_bubble, new_bubble_force)) {
    // error during calculation of forces for the new position - discard changes
    ROS_DEBUG("Cannot calculate forces on bubble %d at new position - "
              "discarding changes",
              bubble_num);
    return true;
  }

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Check for zero-crossings in force on bubble %d", bubble_num);
#endif

  // check for zero-crossings in the force-vector
  double checksum_zero, abs_new_force, abs_old_force;

  // project force-vectors onto each other
  checksum_zero =
      (new_bubble_force.wrench.force.x * forces.at(bubble_num).wrench.force.x) +
      (new_bubble_force.wrench.force.y * forces.at(bubble_num).wrench.force.y) +
      (new_bubble_force.wrench.torque.z *
       forces.at(bubble_num).wrench.torque.z);

  // if sign changes and ...
  if (checksum_zero < 0.0) {
    ROS_DEBUG("Detected zero-crossings in force on bubble %d. Checking total "
              "change in force.",
              bubble_num);
    // check the absolute values of the two vectors
    abs_new_force = sqrt(
        (new_bubble_force.wrench.force.x * new_bubble_force.wrench.force.x) +
        (new_bubble_force.wrench.force.y * new_bubble_force.wrench.force.y) +
        (new_bubble_force.wrench.torque.z * new_bubble_force.wrench.torque.z));
    abs_old_force = sqrt((forces.at(bubble_num).wrench.force.x *
                          forces.at(bubble_num).wrench.force.x) +
                         (forces.at(bubble_num).wrench.force.x *
                          forces.at(bubble_num).wrench.force.x) +
                         (forces.at(bubble_num).wrench.torque.z *
                          forces.at(bubble_num).wrench.torque.z));

    // force still has a significant high value (> ~75% of old force by default)
    if ((abs_new_force > equilibrium_relative_overshoot_ * abs_old_force) &&
        (abs_new_force > significant_force_)) {
      ROS_DEBUG("Detected significante change in force (%f to %f) on bubble "
                "%d. Entering Recursive Approximation.",
                abs_old_force, abs_new_force, bubble_num);
      // o.k. now we really have to take a closer look -> start recursive
      // approximation to equilibrium-point
      int curr_recursion_depth = 0;
      geometry_msgs::Twist new_step_width;
      Bubble curr_bubble = band.at(bubble_num);
      geometry_msgs::WrenchStamped curr_bubble_force = forces.at(bubble_num);

      // half step size
      new_step_width.linear.x = 0.5 * bubble_jump.linear.x;
      new_step_width.linear.y = 0.5 * bubble_jump.linear.y;
      new_step_width.linear.z = 0.5 * bubble_jump.linear.z;
      new_step_width.angular.x = 0.5 * bubble_jump.angular.x;
      new_step_width.angular.y = 0.5 * bubble_jump.angular.y;
      new_step_width.angular.z = 0.5 * bubble_jump.angular.z;

      // one step deeper into the recursion
      if (moveApproximateEquilibrium(bubble_num, band, curr_bubble,
                                     curr_bubble_force, new_step_width,
                                     curr_recursion_depth)) {
        // done with recursion - change bubble and hand it back
        ROS_INFO("33[1;33mdeeper changed new bubble33[0m");
        new_bubble = curr_bubble;


#ifdef DEBUG_EBAND_
        geometry_msgs::Pose2D curr_bubble_pose2D;
        PoseToPose2D(curr_bubble.center.pose, curr_bubble_pose2D);
        ROS_DEBUG("Instead - Try moving bubble %d at (%f, %f, %f) by (%f, %f, "
                  "%f) to (%f, %f, %f).",
                  bubble_num, bubble_pose2D.x, bubble_pose2D.y,
                  bubble_pose2D.theta, new_step_width.linear.x,
                  new_step_width.linear.y, new_step_width.angular.z,
                  curr_bubble_pose2D.x, curr_bubble_pose2D.y,
                  curr_bubble_pose2D.theta);
#endif
      }
    }
  }

  // check validity of resulting band (given the moved bubble)

  // TODO use this routine not only to check whether gap can be filled but also
  // to fill gap (if possible) get local copy of band, set new position of moved
  // bubble and init iterators
  std::vector<Bubble> tmp_band = band;
  std::vector<Bubble>::iterator start_iter, end_iter;
  tmp_band.at(bubble_num) = new_bubble;
  start_iter = tmp_band.begin();

  // check left connection (bubble and bubble-1)
  start_iter = start_iter + bubble_num - 1;
  end_iter = start_iter + 1;

  // check Overlap - if bubbles do not overlap try to fill gap
  if (!checkOverlap(*start_iter, *end_iter)) {
    if (!fillGap(tmp_band, start_iter, end_iter)) {
      ROS_DEBUG("Bubble at new position cannot be connected to neighbour. "
                "Discarding changes.");
      // this bubble must not be changed, but band is still valid -> continue
      // with other bubbles
      return true;
    }
  }

  // get fresh copy of band, set new position of bubble again and reinit
  // iterators
  tmp_band = band;
  tmp_band.at(bubble_num) = new_bubble;
  start_iter = tmp_band.begin();

  // check right connection (bubble and bubble +1)
  start_iter = start_iter + bubble_num;
  end_iter = start_iter + 1;

  // check Overlap - if bubbles do not overlap try to fill gap
  if (!checkOverlap(*start_iter, *end_iter)) {
    if (!fillGap(tmp_band, start_iter, end_iter)) {
      ROS_DEBUG("Bubble at new position cannot be connected to neighbour. "
                "Discarding changes.");
      // this bubble must not be changed, but band is still valid -> continue
      // with other bubbles
      return true;
    }
  }

  // check successful - bubble and band valid apply changes

#ifdef DEBUG_EBAND_
  ROS_DEBUG("Frame %d of %d: Check successful - bubble and band valid. "
            "Applying Changes",
            bubble_num, ((int)band.size()));
#endif

  return true;
}

结语

eband 系列解析到这里差不多就结束了，个人感觉 eband 规划器代码的逻辑非常清晰，作者的注释也非常详细，大家有空的话值得研究一下。

大家也可以发现，相比于 dwa，teb 等等规划器, eband 的参数是非常少的，这也减轻了大家调参的负担，稍微调一调基本上就可以得到比较好的结果。如果使用全向底盘，推荐大家试试。

最后

以上就是爱笑毛巾为你收集整理的eband_local_planner源码解析（7）eband_local_planner.h/cpp 解读eband_->setplan()eband_->optimizeBand()结语的全部内容，希望文章能够帮你解决eband_local_planner源码解析（7）eband_local_planner.h/cpp 解读eband_->setplan()eband_->optimizeBand()结语所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。