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

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_;

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.

Confirmed speedup with VTune. Very nice approach.

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

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

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

@mikeferguson, back to C++03

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

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;
}

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.

@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.

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.

Something else blocking merge?

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

any updates on status of this PR?

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

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

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

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?

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.

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

@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.

@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.

@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.

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.

@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.

@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.

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.

@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" : )

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

Hi all

Has this speedup on inflation layer update implemented in ROS Noetic navigation stack deb?

Best,
Samuel

Hi all

Has this speedup on inflation layer update implemented in ROS Noetic navigation stack deb?

Best, Samuel

hello samuel， I have the same question in ros melodic version!!!

anyone can reply?

Hi @wanghuohuo0716

Currently these features only available on ROS2.

Best,
Samuel

Hi all

Has this speedup on inflation layer update implemented in ROS Noetic navigation stack deb?

Th initial 60% yes, both in melodic and noetic. The additional 25% only in ROS2, as @HappySamuel says
But you are compiling ROS nav from source, try simple to rebase the branch into yours, or cherry-pick the commits

ok, thx!!! I will try it~