Fixing the offset that exists between the grid and the raw data

[KavenYau]

Fixed https://github.com/SteveMacenski/spatio_temporal_voxel_layer/issues/224.

Without this patch:

With this patch:

[SteveMacenski]

My sincerest apologies for the delay, this really wasn't that complicated and I kept pushing it off.

[doisyg]

Ok, quickly jumping in, I am a bit confused. Is this supposed to be only porting to Foxy the fix I did for Galactic there : https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/213 or there is something more than a visualization issue to fix?

EDIT: reading the full conversation

[SteveMacenski]

I think what the author has pointed out is that the previous solution didn’t actually fully fix the issue and this does.

While this is targeted at Foxy, if you agree with these changes, I would update all ROS2 branches with this change and revert the previous fix.

While visualization is part of it, if the grid is offset to the costmap, then that would have a real world impact of a half cell shift of measurements vs costmap cells occupied potentially

[doisyg]

I think what the author has pointed out is that the previous solution didn’t actually fully fix the issue and this does.

Yes I just read the comments.

There is maybe an issue with my fix when costmap_resoltion != voxel_size, but not sure this PR completely fixes it. I would like to take the time to run a couple of tests. I willl post here in the following days comparative results with different conditions and code version. In the meantime, @KavenYau, can you clarify a bit more what you believe the original issue is and in which conditions https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/213 is not enough ? I propose we keep the following convention when displaying results (as in https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/213 ):

• lethal pixels are in pink
• input stvl sensor pointcloud is the small blue spheres
• voxel visualization is in green and displayed as boxes with alpha 0.4 and of size equal to pixel size
• nothing else is displayed (i.e. no inflation)
• view is TopDownOrtho to avoid false inference due to perspective
• zoomed enough to clearly see misalignement

For instance the following example shows misalignment of voxel visualization (size 0.05) against the costmap grid and the input pointcloud:

[KavenYau]

can you clarify a bit more what you believe the original issue is and in which conditions https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/213 is not enough ?

I think there are 3 points:

1. I guess tests in #213 did not cover all quadrants.

2. 213 is assuming the lethal pixels are correct. But I made a small experiment and the result shows that it's not, all the numbers in one of the directions(positive or negative) on each axis have an offset. Described in https://github.com/SteveMacenski/spatio_temporal_voxel_layer/issues/224#issuecomment-1040951516.

3. As #213 mentioned, when costmap_resoltion is equals to voxel_size, the pointcloud where the x and y coordinates of each point are the coordinates of the corner of the voxel. Below figure shows that the lethal grids of costmap tend to extend towards the origin ( like yellow arrows).

I propose we keep the following convention when displaying results (as in https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/213 ):

Ok, but currently I haven't the environment. I will create it if necessary.

[doisyg]

Sorry for the late reply, I was busy and wanted to do some proper testing on a real hardware. I can confirm that https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/213 is not fixing the issue when costmap_resoltion != voxel_size (current galactic branch) I did some tests on the galactic branch (my HW environment) with a revert of https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/213 and by applying this PR. It seems to fix the issue for all cases. Tests done with a sensor generating pointclouds (and not laserscan) with a limited fov, hence only on the 2 forward quadrant. I did not do more digging by this looks good to me. Nice catch @KavenYau !

[SteveMacenski]

OK - just to be clear before I do this, you're OK if I:

• Revert #213 for all branches
• Apply this for all branches

[doisyg]

Yes !