eband_local_planner学习1

最新推荐文章于 2023-04-21 08:52:18 发布

Jaky_wang

最新推荐文章于 2023-04-21 08:52:18 发布

阅读量1.7k

点赞数 1

分类专栏： eband_local_planner

本文链接：https://blog.csdn.net/WangQiang319670/article/details/78910828

版权

eband_local_planner 专栏收录该内容

2 篇文章 1 订阅

订阅专栏

eband_local_planner_ros.h代码：
公有函数：
默认构造函数　　　　　　EBandPlannerROS();
ros包装构造函数　　　　EBandPlannerROS(std::string name, tf::TransformListener* tf,　costmap_2d::Costmap2DROS* costmap_ros);
析构函数　　　　　　　　~EBandPlannerROS();

　　　　　　
包装初始化函数　void initialize(std::string name, tf::TransformListener* tf,　costmap_2d::Costmap2DROS* costmap_ros);

设置Plan 　　　　　　　bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
计算速度　　　　　　　bool　computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
检查是否到达目标　　　bool isGoalReached();

私有成员变量：
指向costmap的指针　　costmap_2d::Costmap2DROS* costmap_ros_;
指向tf监听的指针　　　tf::TransformListener* tf_;
目标点容忍参数　　　　double yaw_goal_tolerance_,xy_goal_tolerance_;
速度绝对值的下界　　　double rot_stopped_vel_, trans_stopped_vel_;

发布和订阅：

   　 ros::Publisher g_plan_pub_; ///<@brief publishes modified global plan
      ros::Publisher l_plan_pub_; ///<@brief publishes prediction for local commands
      ros::Subscriber odom_sub_; ///<@brief subscribes to the odometry topic in global namespace

数据：

  nav_msgs::Odometry base_odom_;
  std::vector`<`geometry_msgs::PoseStamped`>` global_plan_; // plan as handed over from move_base or global planner
  std::vector`<`geometry_msgs::PoseStamped`>` transformed_plan_; // plan transformed into the map frame we are working in
  std::vector`<`int`>` plan_start_end_counter_; // stores which number start and end frame of the transformed plan have inside the global plan

智能指针：

  boost::shared_ptr`<`EBandPlanner`>` eband_;
  boost::shared_ptr`<`EBandVisualization`>` eband_visual_;
  boost::shared_ptr`<`EBandTrajectoryCtrl`>` eband_trj_ctrl_;

flag:
　　bool goal_reached_; 　　 bool initialized_;
锁：
　　boost::mutex odom_mutex_; // mutex to lock odometry-callback while data is read from topic

回调函数：
　　void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);

eband_local_planner_ros.cpp代码：
默认构造函数　　　 EBandPlannerROS::EBandPlannerROS() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {}
ros包装构造函数　　　
EBandPlannerROS::EBandPlannerROS(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) : costmap_ros_(NULL), tf_(NULL), initialized_(false) { // initialize planner initialize(name, tf, costmap_ros);　　　//调用了initialize函数 }
析构函数　EBandPlannerROS::~EBandPlannerROS() {}
初始化函数

    void EBandPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
    {
      // check if the plugin is already initialized
      if(!initialized_)
      {
        // copy adress of costmap and Transform Listener (handed over from move_base)
        costmap_ros_ = costmap_ros;
        tf_ = tf;


        // create Node Handle with name of plugin (as used in move_base for loading)
        ros::NodeHandle pn("~/" + name);

        // read parameters from parameter server
        // get tolerances for "Target reached"
        pn.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
        pn.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1);

        // set lower bound for velocity -> if velocity in this region stop! (to avoid limit-cycles or lock)
        pn.param("rot_stopped_vel", rot_stopped_vel_, 1e-2);
        pn.param("trans_stopped_vel", trans_stopped_vel_, 1e-2);

        // advertise topics (adapted global plan and predicted local trajectory)
        g_plan_pub_ = pn.advertise<nav_msgs::Path>("global_plan", 1);
        l_plan_pub_ = pn.advertise<nav_msgs::Path>("local_plan", 1);


        // subscribe to topics (to get odometry information, we need to get a handle to the topic in the global namespace)
        ros::NodeHandle gn;
        odom_sub_ = gn.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&EBandPlannerROS::odomCallback, this, _1));


        // create the actual planner that we'll use. Pass Name of plugin and pointer to global costmap to it.
        // (configuration is done via parameter server)
        eband_ = boost::shared_ptr<EBandPlanner>(new EBandPlanner(name, costmap_ros_));

        // create the according controller
        eband_trj_ctrl_ = boost::shared_ptr<EBandTrajectoryCtrl>(new EBandTrajectoryCtrl(name, costmap_ros_));

        // create object for visualization
        eband_visual_ = boost::shared_ptr<EBandVisualization>(new EBandVisualization);

        // pass visualization object to elastic band
        eband_->setVisualization(eband_visual_);

        // pass visualization object to controller
        eband_trj_ctrl_->setVisualization(eband_visual_);

        // initialize visualization - set node handle and pointer to costmap
        eband_visual_->initialize(pn, costmap_ros);


        // set initialized flag
        initialized_ = true;

        // this is only here to make this process visible in the rxlogger right from the start
        ROS_DEBUG("Elastic Band plugin initialized.");
      }
      else
      {
        ROS_WARN("This planner has already been initialized, doing nothing.");
      }
    }

setPlan函数

　　　　// set global plan to wrapper and pass it to eband
   　　　 bool EBandPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_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;
      　　}
      　　//reset the global plan
      global_plan_.clear();
      global_plan_ = orig_global_plan;

      // transform global plan to the map frame we are working in
      // this also cuts the plan off (reduces it to local window)
      std::vector<int> start_end_counts (2, (int) global_plan_.size()); // counts from the end() of the plan
      //  start_end_counts    68    1    　　68为global_plan_的size
      //  global_plan_       transformed_plan_    目前看来数据一模一样
      if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, start_end_counts))
      {
        // if plan could not be tranformed abort control and local planning
        ROS_WARN("Could not transform the global plan to the frame of the controller");
        return false;
      }

      // also check if there really is a plan
      if(transformed_plan_.empty())
      {
        // if global plan passed in is empty... we won't do anything
        ROS_WARN("Transformed plan is empty. Aborting local planner!");
        return false;
      }

      // set plan - as this is fresh from the global planner robot pose should be identical to start frame
      if(!eband_->setPlan(transformed_plan_))
      {
        // We've had some difficulty where the global planner keeps returning a valid path that runs through an obstacle
        // in the local costmap. See issue #5. Here we clear the local costmap and try one more time.
        costmap_ros_->resetLayers();
        if (!eband_->setPlan(transformed_plan_)) {
          ROS_ERROR("Setting plan to Elastic Band method failed!");
          return false;
        }
      }
      ROS_DEBUG("Global plan set to elastic band for optimization");

      // plan transformed and set to elastic band successfully - set counters to global variable
      plan_start_end_counter_ = start_end_counts;

      // let eband refine the plan before starting continuous operation (to smooth sampling based plans)
      eband_->optimizeBand();


      // display result
      std::vector<eband_local_planner::Bubble> current_band;
      if(eband_->getBand(current_band))
        eband_visual_->publishBand("bubbles", current_band);

      // set goal as not reached
      goal_reached_ = false;

      return true;
    }

setplan函数具体分析：
１．调用了函数transform global plan to the map frame we are working in，（但是发现global_plan_[0].header.frame_id和costmap_ros_->getGlobalFrameID() 都为/map，故不明白）

eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, start_end_counts)

函数transformGlobalPlan：

bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan, 
      costmap_2d::Costmap2DROS& costmap, const std::string& global_frame, 
      std::vector<geometry_msgs::PoseStamped>& transformed_plan, std::vector<int>& start_end_counts)
  {
    const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

    // initiate refernce varibles
    transformed_plan.clear();

    try
    {
      if (!global_plan.size() > 0)
      {
        ROS_ERROR("Recieved plan with zero length");
        return false;
      }

      tf::StampedTransform transform;
      tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, 
          plan_pose.header.frame_id, transform); //转换，但坐标系都在/map????
      std::cout <<"plan_pose.header.frame_id   " << plan_pose.header.frame_id  << std::endl;   //map
      std::cout <<"global_frame   " << global_frame  << std::endl; //map
      //fixed_frame The frame in which to assume the transform is constant in time. 
      //let's get the pose of the robot in the frame of the plan
      tf::Stamped<tf::Pose> robot_pose;
      robot_pose.setIdentity();
      robot_pose.frame_id_ = costmap.getBaseFrameID();   //robot_base_frame_
      robot_pose.stamp_ = ros::Time();
      tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);

      //we'll keep points on the plan that are within the window that we're looking at

      double dist_threshold = std::max(
          costmap.getCostmap()->getSizeInMetersX() / 2.0,
          costmap.getCostmap()->getSizeInMetersY() / 2.0
          );
//costmap.getCostmap()->getSizeInMetersX()为地图的ｘ轴长度
      unsigned int i = 0;
      double sq_dist_threshold = dist_threshold * dist_threshold;
      double sq_dist = DBL_MAX;

      // --- start - modification w.r.t. base_local_planner
      // initiate start_end_count
      std::vector<int> start_end_count;
      start_end_count.assign(2, 0);

      // we know only one direction and that is forward! - initiate search with previous start_end_counts
      // this is neccesserry to work with the sampling based planners - path may severall time enter and leave moving window
      ROS_ASSERT( (start_end_counts.at(0) > 0) && (start_end_counts.at(0) <= (int) global_plan.size()) );
      i = (unsigned int) global_plan.size() - (unsigned int) start_end_counts.at(0);
      // --- end - modification w.r.t. base_local_planner

      //we need to loop to a point on the plan that is within a certain distance of the robot
      while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold)
      {
        double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
        double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
        sq_dist = x_diff * x_diff + y_diff * y_diff;

        // --- start - modification w.r.t. base_local_planner
        // not yet in reach - get next frame
        if( sq_dist > sq_dist_threshold )
          ++i;
        else
          // set counter for start of transformed intervall - from back as beginning of plan might be prunned
          start_end_count.at(0) = (int) (((unsigned int) global_plan.size()) - i);
        // --- end - modification w.r.t. base_local_planner
      }


      tf::Stamped<tf::Pose> tf_pose;
      geometry_msgs::PoseStamped newer_pose;

      //now we'll transform until points are outside of our distance threshold
      while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold)
      {
        double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
        double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
        sq_dist = x_diff * x_diff + y_diff * y_diff;

        const geometry_msgs::PoseStamped& pose = global_plan[i];
        poseStampedMsgToTF(pose, tf_pose);
        tf_pose.setData(transform * tf_pose);
        tf_pose.stamp_ = transform.stamp_;
        tf_pose.frame_id_ = global_frame;
        poseStampedTFToMsg(tf_pose, newer_pose);

        transformed_plan.push_back(newer_pose);

        // --- start - modification w.r.t. base_local_planner
        // set counter for end of transformed intervall - from back as beginning of plan might be prunned
        start_end_count.at(1) = int (((unsigned int) global_plan.size()) - i);
        // --- end - modification w.r.t. base_local_planner

        ++i;
      }

      // --- start - modification w.r.t. base_local_planner
      // write to reference variable
      start_end_counts = start_end_count;
      // --- end - modification w.r.t. base_local_planner
    }
    catch(tf::LookupException& ex)
    {
      ROS_ERROR("No Transform available Error: %s\n", ex.what());
      return false;
    }
    catch(tf::ConnectivityException& ex)
    {
      ROS_ERROR("Connectivity Error: %s\n", ex.what());
      return false;
    }
    catch(tf::ExtrapolationException& ex)
    {
      ROS_ERROR("Extrapolation Error: %s\n", ex.what());
      if (global_plan.size() > 0)
        ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());

      return false;
    }

    return true;
  }

2.调用了eband_->setPlan(transformed_plan_)

  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
    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     将路径转换为bubbles，计算bubbles的中心和膨胀
    ROS_DEBUG("Converting Plan to Band");
    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_))   //优化bubbles
    {
      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;
  }

其中调用了函数convertPlanToBand(global_plan_, elastic_band_)和refineBand(elastic_band_)

bool EBandPlanner::convertPlanToBand(std::vector<geometry_msgs::PoseStamped> plan, 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;
    }

    // create local variables
    double distance = 0.0;
    std::vector<Bubble> tmp_band;

    ROS_DEBUG("Copying plan to band - Conversion started: %d frames to convert.", ((int) plan.size()) );

    // get local copy of referenced variable
    tmp_band = band;

    // adapt band to plan
    tmp_band.resize(plan.size());
    for(int i = 0; i < ((int) plan.size()); i++)
    {
#ifdef DEBUG_EBAND_
      ROS_DEBUG("Checking Frame %d of %d", i, ((int) plan.size()) );
#endif

      // set poses in plan as centers of bubbles
      tmp_band[i].center = plan[i];

      // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment]
      if(!calcObstacleKinematicDistance(tmp_band[i].center.pose, distance))
      {
        // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
        ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d outside map", i, ((int) plan.size()) );
        return false;
      }

      if(distance <= 0.0)
      {
        // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
        ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d in collision. Plan invalid", i, ((int) plan.size()) );
        // TODO if frame in collision try to repair band instaed of aborting averything
        return false;
      }


      // assign to expansion of bubble
      tmp_band[i].expansion = distance;
    }

    // write to referenced variable
    band = tmp_band;

    ROS_DEBUG("Successfully converted plan to band");
    return true;
  }

bool EBandPlanner::convertPlanToBand(std::vector<geometry_msgs::PoseStamped> plan, 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;
    }

    // create local variables
    double distance = 0.0;
    std::vector<Bubble> tmp_band;

    ROS_DEBUG("Copying plan to band - Conversion started: %d frames to convert.", ((int) plan.size()) );

    // get local copy of referenced variable
    tmp_band = band;

    // adapt band to plan
    tmp_band.resize(plan.size());
    for(int i = 0; i < ((int) plan.size()); i++)
    {
#ifdef DEBUG_EBAND_
      ROS_DEBUG("Checking Frame %d of %d", i, ((int) plan.size()) );
#endif

      // set poses in plan as centers of bubbles
      tmp_band[i].center = plan[i];

      // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment]
      if(!calcObstacleKinematicDistance(tmp_band[i].center.pose, distance))
      {
        // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
        ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d outside map", i, ((int) plan.size()) );
        return false;
      }

      if(distance <= 0.0)
      {
        // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
        ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d in collision. Plan invalid", i, ((int) plan.size()) );
        // TODO if frame in collision try to repair band instaed of aborting averything
        return false;
      }


      // assign to expansion of bubble
      tmp_band[i].expansion = distance;
    }

    // write to referenced variable
    band = tmp_band;

    ROS_DEBUG("Successfully converted plan to band");
    return true;
  }

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;
  }

  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;
  }

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

    // calc (kinematic) Distance between bubbles
    double distance = 0.0;
    if(!calcBubbleDistance(bubble1.center.pose, bubble2.center.pose, distance))
    {
      ROS_ERROR("failed to calculate Distance between two bubbles. Aborting check for overlap!");
      return false;
    }

    // compare with size of the two bubbles
    if(distance >= min_bubble_overlap_ * (bubble1.expansion + bubble2.expansion))
      return false;

    // TODO this does not account for kinematic properties -> improve

    // everything fine - bubbles overlap
    return true;
  }

3.调用了eband_->optimizeBand()
实际调用函数如下：

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);

      // 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;
  }