Add costmap downsampler - fixes SteveMacenski/navigation2#4

[carlosluis]

Basic Info

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

---

Description of contribution in a few bullet points

• Added a CostmapDownsampler that creates a new costmap with the specified resolution. In short, this downsampler inspects the subcells of the original costmap and assigns the highest cost to the corresponding new downsampled cell.
• Added an overlay in Rviz to visualize this map, there's an example below

Example with Willow world

Ran a few tests in the willow world, here's how the map looks after being downsampled from a 0.1m resolution to 0.3m

Comparison	Resolution (m)	Nodes expanded	Avg runtime (ms)
0.1	67420	~160
0.3	5460	~20

Runtime impact: downsampling the map is only done once (the first time we create a path) and it takes around 5ms for a 566x108 cells map. It's computation time is vastly outweighted by the amount of less nodes expanded.

Description of documentation updates required from your changes

• Added two new parameters for the smac planner: boolean for using the downsampler, and a float specifying the desired resolution for downsampling.

---

Future work that may be required

• Could consider other ways to downsample aside from just picking up the max
• Create test-suite for the downsampler. I could probably include this in this PR or leave it for #9

[SteveMacenski]

In you image, I still see small dots - are those from an underlaying full resolution map? Can you give us a screen shot of just the downsampled map? I'm surprised to see black and some smaller cells with small gradients. I also don't think LETHAL is blue (but I could be wrong), I don't see LETHAL's over the small black cells, are you sure that is correct? I would think all the black cells for static obstacles in the map should be overlapped in the downsampled costmap with lethal coloring. I think lethal is red not the light blue.

[carlosluis]

The image had the full resolution map beneath, you're right. Here's one with only the downsampled map

I just checked and the blue color correspond to an inscribed obstacle (cost = 253), while the magenta is lethal (cost = 254).

I verified that every black dot in the full resolution map actually corresponds to lethal cost, so I think the downsampling is correct.

[SteveMacenski]

Got it, thanks! Met me re-review

[carlosluis]

I was able to address most comments:

• Moved costmap downsampler and publisher initializations to SmacPlanner::configure(). I thought I had problems with that setup, but it ended up working fine
• Now the costmap downsampler supports the case when the costmap is changing sizes (e.g. the SLAM example you mentioned). If the costmap changes size then we call the resize function. I tested this feature by setting a dummy costmap of 1x1 cell in the downsampler initialization, which then got corrected to the actual size needed when calling the downsample function.

[carlosluis]

Moved the publisher to the downsampler. I think the code looks a bit cleaner now. Let me know what you think!

I've tested again that the publisher is correctly updating the costmap. I initialized with some dummy value (a 1x1 costmap), published that costmap, and then it gets correctly updated when calling downsample

[SteveMacenski]

Looks more basic now - that's a good sign. When it looks simple, that's the hallmark of a good design. I'm wondering if we should make the downsample factor an input to downsample(). It seems natural to tell it what to downsample by and would make our lives easier for testing if we could just flip a varable range over a loop to an object to see the results.

[SteveMacenski]

Alright, we're down to pedantic stuff, I think this is the last few changes and we're good to go

[carlosluis]

For some reason I pushed to my branch https://github.com/carlosluisg/navigation2/tree/costmap_downsampler but it is not showing in the PR. I addressed there the last few comments

EDIT: registered just now :) seemed like Github had a small hiccup

[SteveMacenski]

OK I think this can go in now. Anything else you want to change?

Only 2 things I see that are really small would be (optional):

• The min() will continue to iterate over maxed out cells instead of wrapping over, we could see if they're in bounds and continue; if not so we avoid some more calls.
• The set costs look could be 2 nested for loops so that each iteration you're only updating 1 variable rather than 2. Right now each of the N*N iterations you find the x and y coordinate. If you format as 2 for loops then each one you can set 1 variable so that you have less total mod and casting operations.

[carlosluis]

Last commit should address the 2 comments you mentioned last.

If you agree with those changes, I have nothing else to add to this PR and it can be merged.

[carlosluis]

Reverted back to using continue

[SteveMacenski]

Awesome! All good, merging. This was a really great step forward to restarting this work, thanks for working with me on performance items. This planner will naturally be slower than a usual A* so I'm hyper sensitive about making sure to reduce any small overhead that could accumulate.