Ask Your Question
1

prunePlan() takes occasionally over 40 seconds!

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

madmax gravatar image

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

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

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

Wolf gravatar image

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

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() );
edit flag offensive delete link more

Comments

1

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 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

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

Seen: 431 times

Last updated: Jan 23 '14