Use double buffering in particle storage

[hidmic]

Proposed changes

Pretty much what the title says. This patch fully initializes a particle cloud in the background before replacing the one in foreground.

Type of change

• 🐛 Bugfix (change which fixes an issue)
• 🚀 Feature (change which adds functionality)
• 📚 Documentation (change which fixes or extends documentation)

Checklist

• Lint and unit tests (if any) pass locally with my changes
• I have added tests that prove my fix is effective or that my feature works
• I have added necessary documentation (if appropriate)
• All commits have been signed for DCO

Additional comments

The rationale for double buffering stems from the use of ranges for particle cloud initialization during resampling. Since range evaluation is lazy, the resampling algorithm would end up reading from the same particle set it is mutating, skewing the results.

Nav2 AMCL is careful about this. We were not (and that's how I realized).

[glpuga]

Great catch! LGTM

[nahueespinosa]

LGTM!