Add option to inflate unknown space

[shrijitsingh99]

Signed-off-by: Shrijit Singh shrijitsingh99@gmail.com

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1628
Primary OS tested on	Ubuntu
Robotic platform tested on	turtlebot simulation

---

Description of contribution in a few bullet points

• Added option to inflate unknown space
• Made some small optimizations and changes to make code cleaner

Description of documentation updates required from your changes

• Added new parameter inflate_around_unknown, so need to add that to default configs and documentation page

[codecov]

Codecov Report

Merging #1675 into master will increase coverage by 0.96%.
The diff coverage is 98.83%.

@@            Coverage Diff             @@
##           master    #1675      +/-   ##
==========================================
+ Coverage   63.41%   64.37%   +0.96%     
==========================================
  Files         192      192              
  Lines       10321    10334      +13     
==========================================
+ Hits         6545     6653     +108     
+ Misses       3776     3681      -95     

Flag	Coverage Δ
#project	64.37% <98.83%> (+0.96%)	⬆️

Impacted Files	Coverage Δ
...map_2d/include/nav2_costmap_2d/inflation_layer.hpp	91.66% <92.30%> (+5.95%)	⬆️
...tmap_2d/include/nav2_costmap_2d/testing_helper.hpp	78.57% <100.00%> (ø)
nav2_costmap_2d/plugins/inflation_layer.cpp	98.12% <100.00%> (+1.62%)	⬆️
nav2_costmap_2d/src/layered_costmap.cpp	88.57% <100.00%> (+1.42%)	⬆️
nav2_amcl/src/amcl_node.cpp	83.23% <0.00%> (-0.77%)	⬇️
nav2_navfn_planner/src/navfn.cpp	81.07% <0.00%> (+0.21%)	⬆️
nav2_costmap_2d/plugins/obstacle_layer.cpp	81.37% <0.00%> (+0.34%)	⬆️
nav2_costmap_2d/src/costmap_layer.cpp	37.63% <0.00%> (+2.15%)	⬆️
... and 4 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 13633cd...5f2ddae. Read the comment docs.

[SteveMacenski]

ros-planning/navigation#839

This may also be good to include if you're interested in the inflation layer. Porting this to ROS2 would speed things up quite a bit.

[SteveMacenski]

I don't understand a bunch of these changes. Please explain. They don't see to follow the speed up PR or relate to inflating unknown space.

[shrijitsingh99]

I don't understand a bunch of these changes. Please explain. They don't see to follow the speed up PR or relate to inflating unknown space.

Since I was already working on the speedup you mentioned, I added some more optimizations and in general just cleaned up some stuff to be inline with modern practices:

• Convert cached_costs_ and cached_distances_ to vectors, they allow for automatic memory management, no need for new and delete.
• Convert cached_costs_ and cached_distances_ to 1D vectors, to allow better locality of reference.
• Introduced explicit static casts in-place of implicit and C-Styled casts and removed unnecessary casts.
• Added override keyword to mark implicit overriding of virtual functions.
• Fixed a issue mentioned in the code comments regarding -numeric_limit<double>::max()

[SteveMacenski]

There are a bunch of missing changes that Jorge implemented. At the bare minimum you're missing https://github.com/ros-planning/navigation2/blob/d21cf3366b06e3a63014e411708c2cbae2ae3647/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp#L171 the change between a map to a vector, which I think accounts for the vast majority of the speed up.

I also don't see the eq. of the seen queue. Honestly, I'm not seeing alot of analog here. I think you should simplify to a direct port before trying to do anything else. Some of the changes you made actually will make the code a little slower.

[shrijitsingh99]

There are a bunch of missing changes that Jorge implemented. At the bare minimum you're missing

https://github.com/ros-planning/navigation2/blob/d21cf3366b06e3a63014e411708c2cbae2ae3647/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp#L171
the change between a map to a vector, which I think accounts for the vast majority of the speed up.

I have that port ready, haven't pushed it currently.

I think you should simplify to a direct port before trying to do anything else. Some of the changes you made actually will make the code a little slower.
I mostly made changes related to recommended C+= syntax so shouldn't negatively affect performance.

Haven't benchmarked the cached_costs_ and cached_distances_ but should be slightly faster due to locality, worst case there is no performance impact.

Might have missed some stuff causing it to be slower. If you think all the changes are unneeded, will revert back.

[SteveMacenski]

I would make all the changes he made. Jorge's a smart guy and if those changes were robustly checked from months of running on Magazino's robots, I'd go with that and take his changes at face value.

[SteveMacenski]

This looks good to me, can you verify you see a ~20% cpu drop?

[SteveMacenski]

@shrijitsingh99 it fails some linters and the unit tests for the inflation layer. Please make sure to run these tests locally + run the stack with these change to debug/test/get a feel for changes. It looks like there's a real bug in there.

[shrijitsingh99]

Yeah sorry about that. Have made separate issue for a bug in the tests. Lets get that merged first before proceeding with this. Opened a separate PR since I don't think it is a good idea modifying the test as well as the module together can lead to bugs creeping in.

Uncrustify seems a little bit erratic. It doesn't show any errors locally but on the CI errors show up.

[SteveMacenski]

@shrijitsingh99 are you running on ROS2 master source build or Eloquent binaries?

You'd see crustify appearing differently if you did Eloquent, there was a master / Foxy update. Its basically just:

auto yo = some_function_long_name(first_thing,
  second_thing);

goes to

auto yo = some_function_long_name(
  first_thing, second_thing);

Any line extensions start arguments on a new line.

[shrijitsingh99]

@shrijitsingh99 are you running on ROS2 master source build or Eloquent binaries?

Eloquent binaries

You'd see crustify appearing differently if you did Eloquent, there was a master / Foxy update.

Yup, pretty much sums the issue I was facing. Will switch to source build.

[SteveMacenski]

@shrijitsingh99 you should rebase to get the new test

[SteveMacenski]

@shrijitsingh99 can you verify you see a CPU drop? I want to make sure we got what we came for with this.

[SteveMacenski]

Want to make sure we see the CPU gains but LGTM

[SteveMacenski]

@shivaang12 please review

[shivaang12]

LGTM. It would be really great if you can add some tests, which is of course optional in case you don't want to. By writing some test (even low level) would be great help!

[SteveMacenski]

Inflation layer is actually one of the best covered files in the repo https://codecov.io/gh/ros-planning/navigation2/src/master/nav2_costmap_2d/plugins/inflation_layer.cpp

Though I think that is mostly from integration testing that wouldn't actually come up with any issues unless it was really messed up since the robot an still navigate as long as its not in the middle of LETHALs.

@shrijitsingh99 that is a good point though. One of the very few uncovered lines is this PR adds https://codecov.io/gh/ros-planning/navigation2/src/master/nav2_costmap_2d/plugins/inflation_layer.cpp#L239 it would be helpful to add a test for checking with this param on / off that it inflates or doesn't inflate unknown space. We're on a bit of a test coverage binge right now if you've followed my recent traffic.

An easy test would be creating a 10x10 costmap of FREE and in the middle a single unknown cell. Check when off that its neighbors are still FREE, when on, checking that they're > FREE.

[shrijitsingh99]

Great, will add some tests for inflate_unknown_ and inflate_around_unknown_.

[SteveMacenski]

@shrijitsingh99 any update on this? We should try to square up existing things before adding new things. 5 unmerged PRs isn't as good as 3 merged ones 😉

You will also need to rebase this on master, a bunch of emergency updates went in over the weekend.

[shrijitsingh99]

@shrijitsingh99 any update on this? We should try to square up existing things before adding new things. 5 unmerged PRs isn't as good as 3 merged ones 😉

I have started work on it should be done by tomorrow morning.

You will also need to rebase this on master, a bunch of emergency updates went in over the weekend.

Will do that 👍

[SteveMacenski]

@shrijitsingh99 did you mess with build settings or setup a CI on your own fork? That build error is not from our stuff and when I click on it it shows as part of your account/fork

[shrijitsingh99]

@shrijitsingh99 did you mess with build settings or setup a CI on your own fork? That build error is not from our stuff and when I click on it it shows as part of your account/fork

Yeah, I was trying to set up the I on my own fork for testing purposes.

I didn't know it will switch to my personal CI for PR's too. How should I do go about resolving this?

[SteveMacenski]

I didn't know it will switch to my personal CI for PR's too. How should I do go about resolving this?

A PR can be thought of as "I have my own version of this [thing] and I want to generate a diff and apply that patch to your [thing]". When the MR is opened, its going to look at your end of things to decide what to trigger/do so it'll naturally run your configs. In fact, you can open a pull request from a completely unrelated repository into this one if you liked and applied some random patch. The GitHub interface doesn't make that easy, but Git will fully allow for it.

tl;dr, change the branch that run your CI stuff against to something other than master or *-devel

[shrijitsingh99]

When the MR is opened, its going to look at your end of things to decide what to trigger/do so it'll naturally run your configs.

I generally have been using Azure Pipelines, it allows to me run my own CI on my fork, and on the PR it runs the upstream repo's CI so I assumed here there was a similar thing.

Is this CircleCI specific or am I missing something?

PS: Have unlinked my own CI, does it need a re-trigger from my end?

[SteveMacenski]

@shrijitsingh99 don't bother, @AlexeyMerzlyakov should, he's been thinking about this problem. Merging this since that's not part of this PR.

---

Alexey, see Ci session below. You can rerun this with SSH and poke around to see what you can find.

[SteveMacenski]

Actually, @shrijitsingh99 can you do one thing on this, can you branch off this branch, rebase on master as of right now, and then resubmit a PR with some trivial change? (add a comment, whatever)

I want to see if this is from some state issue. It can be a draft PR