Configure costmap for the lane lines and occupancy grid

[AbBaSaMo]

• use random/hardcoded occupancy grids to test
• look into how to do this
• get the lane lines integrated
• get the occupancy grid integrated

[AbBaSaMo]

@Jiawei-Liao get this done by friday

[Jiawei-Liao]

I looked into using Nav2 and SLAM to combine the occupancy grids themselves but it doesn’t seem like it’s possible.

For Nav2, it takes just 1 map occupancy grid. I tried adding more to the different plugin layers, obstacle and static but it didn’t seem to work.

For SLAM, it takes other data types like laserscan but not occupancy grids.

So I think a new node needs to be created which uses nav2 layered costmap to combine them into a single occupancy grid.

What do you think of this?

[Jiawei-Liao]

tldr: I got it working.

What I did:

I initially got nav2 to take a different topic other than slam map to create to costmap.

I tried to add multiple static layers, however, this only takes the top layer. I was lead to believe that setting "use_maximum: true" would take the maximum of each layer and combine them. This should prioritize obstacles then free then unknowns. But this is not the case and seems to do nothing.

I thought maybe there could only be 1 static layer node, so I tried to create a duplicate, just under a different name. While I did try to create this in a separate package, there were too many dependencies and I gave up on this. Instead, I just created it adjacent to the default static layer cpp/hpp files. I did get this to compile and run successfully. But it still seems to just use the top layer.

I took a look at the code itself.

It does take in a parameter of use_maximum

And it does use this parameter to take different actions.

I then tested whether is it actually setting use_maximum to true and it turned out it always gets set to false. I have tried setting that variable to be true in the params yaml file. I have also tried setting the default to be true. And also just removing the "set variable" part. Nothing seemed to work. I believe these 2 lines should get it to accept the use_maximum parameter?

But this did not seem to work at all. So instead I removed the checking whether use_maximum is true and just run the code that would occur.
I have commented that check out, so if anyone knows how to fix this param issue, they can be uncommented and let it work like normal.

To add more layers, add plugin and define the layer like so:

While cleaning up the code, I havee noticed that for the local costmap, it is only taking the plugins of voxel_layer and inflation_layer but not statis_layer. However, static layer is defined below. Did you want static layers in the local costmap?

Also what are the topics for lidar? and are we using a provided map? because to run nav2_bringup, I had to provide it with tb3.

[Jiawei-Liao]

With some more tests, it seems like it only works if both occupancy grids are of the same size and resolution. Otherwise, they try to overwrite each other.

Not too sure what to do about this...