Revision history [back]

If you often erase single elements (which you do) from your container I'd suggest to use std::list rather than std::vector. If you erase from std::vector elements other than the end, the entire underlying storage needs to be reallocted, which might take long if its a huge vector...

See ref of std::vector [http://www.cplusplus.com/reference/vector/vector/erase/]:

Because vectors use an array as their underlying storage, erasing elements in positions other than the vector end causes the container to relocate all the elements after the segment erased to their new positions. This is generally an inefficient operation compared to the one performed for the same operation by other kinds of sequence containers (such as list or forward_list).

Note: If you can not change your function signature you could copy to std::list and back before iterating:

// copy to std::list
std::list<geometry_msgs::PoseStamped> my_local_plan( plan.begin(), plan.end() );

// do you iteration stuff here with list 
//.....

// copy back to arg plan.clear();
plan = std::vector<geometry_msgs::PoseStamped>( my_local_plan.begin(), my_local_plan.end() );

If you often erase single elements (which you do) from your container I'd suggest to use std::list rather than std::vector. If you erase from std::vector elements other than the end, the entire underlying storage needs to be reallocted, which might take long if its a huge vector...

See ref of std::vector [http://www.cplusplus.com/reference/vector/vector/erase/]:

Because vectors use an array as their underlying storage, erasing elements in positions other than the vector end causes the container to relocate all the elements after the segment erased to their new positions. This is generally an inefficient operation compared to the one performed for the same operation by other kinds of sequence containers (such as list or forward_list).

Note: If you can not change your function signature you could copy to std::list and back before iterating:

// copy to std::list
std::list<geometry_msgs::PoseStamped> my_local_plan( plan.begin(), plan.end() );

// do you iteration stuff here with list 
//.....

// copy back to arg  plan.clear();
plan = std::vector<geometry_msgs::PoseStamped>( my_local_plan.begin(), my_local_plan.end() my_local_plan.begin(),my_local_plan.end() );