Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Speedup (~60%) inflation layer update #525

Merged
merged 6 commits into from Aug 1, 2017

Conversation

Projects
None yet
8 participants
@corot
Copy link
Contributor

commented Oct 5, 2016

Following this discussion, the pop in the priority_queue used for sorting the cells to inflate takes a lot of CPU. I replaced it by a map containing cells to inflate split by distance from the closest obstacle.

I have played around with the changes for a while, and it seems to work fine, also scaling well to higher resolutions and inflation radius. After profiling my changes, the new biggest CPU eater is the map lookup for distances on enqueue method. Maybe someone with better understanding of STL containers can give an extra speedup, knowing this.

I require C++11. Hope it's not an issue for indigo code.

Jorge Santos added some commits Oct 4, 2016

Jorge Santos
Replace the priority_queue used on inflation layer by a map containin…
…g cells to inflate split by distance from the closest obstacle
@corot

This comment has been minimized.

Copy link
Contributor Author

commented Oct 17, 2016

Any problem on merging this? I think I fixed the failed check in the second commit (doesn't a new commit re-trigger the testing?
Actually, this PR is quite valuable (imho), so I think many people will appreciate to get it merged asap

@procopiostein

This comment has been minimized.

Copy link

commented Oct 24, 2016

it should re-trigger testing, I believe. Looking at the compilation output I found this error, which should be the cause of the failure:
/tmp/catkin_workspace/install_isolated/include/costmap_2d/inflation_layer.h:176:40: error: ‘>>’ should be ‘> >’ within a nested template argument list
08:59:11 std::map<double, std::vector> inflation_cells_;

@corot

This comment has been minimized.

Copy link
Contributor Author

commented Oct 24, 2016

Thanks @procopiostein. I added c++11 to the CMakeLists.txt, so using '>>' on templates should be fine. Maybe this header file used somewhere else where it is compiled with c++98?
I use catkin build, so I build already isolated without any problem.

Anyway, I will just modify the PR by now so it gets merged.

@jasonimercer

This comment has been minimized.

Copy link

commented Oct 31, 2016

Confirmed speedup with VTune. Very nice approach.

inflation_vtune_markup_sanitized

@corot

This comment has been minimized.

Copy link
Contributor Author

commented Nov 2, 2016

Thanks @jasonimercer, nice analysis.
Btw, is the C++11 requirement preventing this to get merged? I think I can get rid of it easily. @AlexReimann pointed in issue #468 that indigo public packages must use C++03

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Nov 2, 2016

C++11 is definitely going to hold up merging this -- if you can get rid of that, I can prioritize testing and merging

@AlexReimann

This comment has been minimized.

Copy link

commented Nov 3, 2016

Also breaks the ABI compatibility probably. I don't think you can get around that. :/

Jorge Santos added some commits Nov 3, 2016

@corot

This comment has been minimized.

Copy link
Contributor Author

commented Nov 3, 2016

@mikeferguson, back to C++03

@AlexReimann

This comment has been minimized.

Copy link

commented Nov 3, 2016

That last commit f8d5aa4 is a bugfix for a bug which is also present in c++11 version?

@jasonimercer

This comment has been minimized.

Copy link

commented Nov 3, 2016

Yes. Confirmed with a little test program:

#include <vector>
#include <iostream>

int main()
{
  std::vector<int> v;

  v.push_back(0);
  v.push_back(1);
  v.push_back(2);

  for (std::vector<int>::iterator it = v.begin(); it != v.end(); it++)
  {
    if (*it == 1)
    {
      v.push_back(5);
      v.push_back(5);
      v.push_back(5);
    }

    std::cout << *it << std::endl;
  }

  return 0;
}
@jasonimercer

This comment has been minimized.

Copy link

commented Nov 3, 2016

and for the C++11 version of the above snippet:

#include <vector>
#include <iostream>

int main()
{
  std::vector<int> v;

  v.push_back(0);
  v.push_back(1);
  v.push_back(2);

  for (auto& i: v)
  {
    if (i == 1)
    {
      v.push_back(5);
      v.push_back(5);
      v.push_back(5);
    }

    std::cout << i << std::endl;
  }

  return 0;
}

it again gives unexpected (but different unexpected) results.

The differences seem to be related to how the termination condition is hoisted.

@corot

This comment has been minimized.

Copy link
Contributor Author

commented Nov 3, 2016

@jasonimercer, thanks for the analysis. Interestingly enough, the code using both iterators and C++11 vector traversing worked, but using iterators makes test crash, hence the second commit. Till now I thought that inserting in a vector doesn't invalidate iterators, but clearly that's not the case.
However, using iterators to traverse the map seems to be fine, even if adding new keys.

@jasonimercer

This comment has been minimized.

Copy link

commented Nov 3, 2016

The above snippets actually work if you only push back a single value as that does not trigger a resize (updated size still fits in vactor capacity). Valgrind doesn't even complain.

When a resize happens the iterator is now stomping through memory no longer related to the vector. If I had a .reserve(big_number) then the iterator version works as intended (until you exceed big_number). The C++11 foreach syntactic sugar still hoists the termination condition and outputs 0 1 2 rather than 0 1 2 5 5 5.

Either way, good catch on the bug fix.

@corot

This comment has been minimized.

Copy link
Contributor Author

commented Nov 8, 2016

Something else blocking merge?

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Nov 9, 2016

@corot -- I'm currently out of the office, but I will look into testing this early next week.

@AravindaDP

This comment has been minimized.

Copy link

commented Jan 25, 2017

any updates on status of this PR?

@corot

This comment has been minimized.

Copy link
Contributor Author

commented May 10, 2017

We have been using costmaps with this change for several months without problem, and the speedup is really welcomed, so I would encourage merging this PR asap

@jasonimercer

This comment has been minimized.

Copy link

commented May 10, 2017

+1
This is solid code now with > 1M hours run time on our robots.

@DLu DLu added the costmap_2d label May 30, 2017

@DLu

This comment has been minimized.

Copy link
Contributor

commented May 30, 2017

This looks good to me, although I'm not sure whether the CellData changes break ABI, and/or whether we should care. 👍

@PPokorski

This comment has been minimized.

Copy link

commented Jul 10, 2017

I'd have another suggestion. Here you could, instead of using push_back (which is a really costly operation) iterate beforehand to determine the size of the vector and then use reserve() or resize(), like:

int number = 0;
for(int j = min_j; j < max_j ; j++) 
   for(int i = min_i; i < max_i; i ++)
       number++;

obs_bin.resize(number)

Btw. How about merging this, anyway?

@jasonimercer

This comment has been minimized.

Copy link

commented Jul 10, 2017

Iterating over the memory twice also has a price and adds more lines of code for someone new to reason about. This can be sped up down the road when C++11 is allowed and we can use move semantics. I'd suggest either merging this PR as-is or closing it. It's high quality, free work by the community given to ROS. ROS should not string contributors along for 9 months.

@procopiostein

This comment has been minimized.

Copy link

commented Jul 28, 2017

@mikeferguson , is there anything stopping this merge? Is there anything we can do to help?

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 28, 2017

@procopiostein I really just need to sit down and test it. Should have time to do that over the next week, trying to do a big scrub of open issues/PRs (I know they've been piling up for a while)

At the end of the day, we're not trying to "string contributors along" -- but we do have day jobs that don't always allow a bunch of time for testing/development. While I'd love to merge lots of new features/improvements quickly, we also have to weigh the fact that when something in navigation breaks (particularly costmaps), real world damage can occur (robots hit stuff). This particular PR drastically changes a core function, and is targeted at an LTS release that was already 2+ years into that 5 year life cycle when the PR was created.

@procopiostein

This comment has been minimized.

Copy link

commented Jul 28, 2017

@mikeferguson thanks for the feedback. I do understand that this is a very delicate PR and it takes a lot of your free time to check it properly. Sorry if it felt I was pressing you in any way, that was not my intention.

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 28, 2017

@procopiostein -- I should have been more clear there -- my first paragraph was responding to your question, but my second paragraph was more general towards the set of the comments that came before yours.

I also didn't specifically answer your question though: one thing you can do to help this PR along would be if you have tested this specific PR on a particular robot platform (real or simulated) please let us know by posting a comment here. It would help to know which robot and which navigation configuration package (if publicly available, please post a link to it). There are many robots running the navigation stack these days, and far more parameters than we can possibly test all combinations, so knowing that something has been tested on a wide set of platforms helps.

@corot

This comment has been minimized.

Copy link
Contributor Author

commented Jul 28, 2017

Thanks for the explanation, @mikeferguson. I really understand how busy core maintainers are.

If this helps, I can PR this on kinetic and/or lunar, so we don't risk our venerable indigo. In any case, we have been using the modified costmaps for almost a year without problems, and in fact got rid off issue #584 that used to happen once in a while before.

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 28, 2017

@corot -- there are still a ton of robots running Indigo -- and it sounds like we do have a bunch of testing going on around this, so I think the community would really like it to go into indigo. We've let it sit here for 9 months -- let me dig into it closer over the next couple days.

@PPokorski

This comment has been minimized.

Copy link

commented Jul 28, 2017

@mikeferguson - As a general remark, I understand how hard it is to find time for maintenance like this, but maybe you could use some help 'hiring' more people for that purpose? It would be a great help for the whole community.

@procopiostein

This comment has been minimized.

Copy link

commented Jul 28, 2017

I did some testing with a real PMB2 robot, running ros indigo and ubuntu 14.04, comparing corot:faster_costmap branch against version 1.12.13.
Everything looks good.
Peaking at the cpu usage, the big difference is while the robot is not navigating, with the cpu at about 40% for corot's branch compared to 120% in 1.12.13.
While moving there was no clear difference in CPU usage.

@jasonimercer

This comment has been minimized.

Copy link

commented Jul 28, 2017

@mikeferguson, sorry I spoke harshly. I know you guys are also contributing your time to manage these repos and keeping the ROS stack stable. I really appreciate all the work you put into it. I just need to recalibrate for upstream and realise that a 2 day old PR isn't "getting old" : )

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Aug 1, 2017

Ok, tested in several hardware configurations here, code looks good. Going to squash-merge for ease of forward port to K+L

@mikeferguson mikeferguson merged commit b2d5079 into ros-planning:indigo-devel Aug 1, 2017

1 check passed

Ipr__navigation__ubuntu_trusty_amd64 Build finished.
Details

mikeferguson added a commit that referenced this pull request Aug 1, 2017

Speedup (~60%) inflation layer update (#525)
* Replace the priority_queue used on inflation layer by a map containing cells to inflate split by distance from the closest obstacle

* Update inflation test to match previous change

mikeferguson added a commit that referenced this pull request Aug 1, 2017

Speedup (~60%) inflation layer update (#525)
* Replace the priority_queue used on inflation layer by a map containing cells to inflate split by distance from the closest obstacle

* Update inflation test to match previous change

gerkey added a commit to codebot/navigation that referenced this pull request Jan 19, 2018

Speedup (~60%) inflation layer update (ros-planning#525)
* Replace the priority_queue used on inflation layer by a map containing cells to inflate split by distance from the closest obstacle

* Update inflation test to match previous change

johaq added a commit to CentralLabFacilities/navigation that referenced this pull request Mar 30, 2018

Speedup (~60%) inflation layer update (ros-planning#525)
* Replace the priority_queue used on inflation layer by a map containing cells to inflate split by distance from the closest obstacle

* Update inflation test to match previous change
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.