ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

prunePlan() takes occasionally over 40 seconds!

asked 2014-01-23 20:47:59 -0600

madmax gravatar image

updated 2014-01-23 22:06:21 -0600


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);
    it = plan.erase(it);
    global_it = global_plan.erase(global_it);

Has anyone an idea what I could do here?


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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2014-01-23 21:10:57 -0600

Wolf gravatar image

updated 2014-01-23 21:16:31 -0600

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 []:

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 = std::vector<geometry_msgs::PoseStamped>( my_local_plan.begin(),my_local_plan.end() );
edit flag offensive delete link more



Thanks for the info! Looks promising! We try this, but also the vector erase version where you can give start and end point to erase from. Maybe that's also faster.

madmax gravatar image madmax  ( 2014-01-23 22:07:46 -0600 )edit

Question Tools

1 follower


Asked: 2014-01-23 20:47:59 -0600

Seen: 472 times

Last updated: Jan 23 '14