Reverted solver blocks removal that crashes multiple initial poses [foxy]

[Kaetera]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#419
Primary OS tested on	Ubuntu 20.04, ROS2 Foxy
Robotic platform tested on	Custom diff drive real robot

---

Description of contribution in a few bullet points

Fixed crashes when setting initial poses, but may re-introduce warnings these lines avoided.

Description of documentation updates required from your changes

N/A

---

Future work that may be required in bullet points

As mentioned in the ticket's discussion this fixes the issue until a "better scan management" is implemented.

[SteveMacenski]

Can you submit across the current branches with it? Just to keep everything aligned. Then I'll reopen #398 with a note about why we reverted it.

[Kaetera]

Done.
I did not figure out how to put commits from several branches in the same PR so I opened #443 and #444 .
Note I could not test the compilation/the run since I only have Foxy installed.