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

[KavenYau]

Fixed #224.

Without this patch:

With this patch:

[SteveMacenski]

I approve of this, but I would like @doisyg to review and also approve as I know he is a user of this and I don't want to break his behavior. Plus he worked on the initial alignment bits so he would be a good person to review this as well.

@doisyg can you review this PR?

If he approves, I'll backport / foward port to all distributions. This would override the changes from the other method used in ros2.

[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 : #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 #213 is not enough ? I propose we keep the following convention when displaying results (as in #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 #213 is not enough ?

I think there are 3 points:

1. I guess tests in Fix voxel visualization #213 did not cover all quadrants.
2. Fix voxel visualization #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 There is an offset between the grid and the raw data #224 (comment).
3. As Fix voxel visualization #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 #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 #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 #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 Fix voxel visualization #213 for all branches
• Apply this for all branches

[doisyg]

Yes !