prunePlan() takes occasionally over 40 seconds!

edit

Hi,

I am wondering how it is possible that prunePlan takes occasionally over 40 seconds? I can't find anything in there that causes this long computation.

It's hard to debug because it doesn't occur that often but we managed at least to localize it to this function:

void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
double global_pose_x = global_pose.getOrigin().x();
double global_pose_y = global_pose.getOrigin().y();
while(it != plan.end()){
    const geometry_msgs::PoseStamped& w = *it;
    // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
    double x_diff = global_pose_x - w.pose.position.x;
    double y_diff = global_pose_y - w.pose.position.y;
    double distance_sq = x_diff * x_diff + y_diff * y_diff;
    if(distance_sq < 1){
        ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
        break;
    }
    it = plan.erase(it);
    global_it = global_plan.erase(global_it);
}
}

Has anyone an idea what I could do here?

Edit:

We also found out that the length of the global plan coming in, has sometimes some "outliers".
For example the planner outputs 800, 750, 600, 15000, 400,... (We compute the path more than once during approaching a goal)

And at this peak the delay is happening.