Speed up improvement in LaserScan and PointCloud2

[facontidavide]

Hi,

it is possible to reduce dramatically the memory allocation in both std::vector::reserve or recycling instances popped from the scan_ buffers.

In my benchmarks, these change roughly reduce the amount of CPU used by these two plugins to 1/3rd.

[evenator]

These look like great changes. I haven't tested them yet, but most of them are common sense memory management. I did note a stylistic gripe with your implementation of the trig cache.

Actually, if you wouldn't mind factoring that out into a separate PR, I think that speed up the process for us to merge this in.

[evenator]

Please avoid the use of local static variables by refactoring these to class variables or creating a new object to encapsulate the entire trigonometry cache.

[facontidavide]

I guess this is just a matter of "personal taste". Or is there any technical reason to avoid static?
No problem, though

[facontidavide]

I understand you point now. You are right

[facontidavide]

EDIT: I stand corrected.
You might indeed have more than 1 laser plugin running.
I believe it would still be safe in most of the cases, but better to play it safe ;)

Cheers and thanks again for the amazing tool.

[facontidavide]

For the records:

1. using std::vector::reserve or std::vector::resize is always good.
2. In laserscan, the main performance killer was mostly the calculation of cos() and sin().
3. the object LaserScanPlugin::Scan is relatively fast to copy in comparison to PointCloud2Plugin::Scan, which is pretty slow.
4. The worst in terms of performance in PointCloud2Plugin was that ANY single point required a memory allocation because of PointCloud2Plugin::StampedPoint::features. Recycling an already allocated instance avoid that.

[pjreed]

This looks good to me. Tested and works in my Kinetic Docker environment. Thanks!