Add center of mass tracking

Hi Pedro - So I took a stab at issue #10 that I filed! I think this does what I need it to do, it essentially publishes a "com_position" event which gets x, y, and z coordinates after you enable it.

Please note: this is the first time I've ever written C++ code so it likely won't meet your standards. Feel free to fix this up if you decide to use it :)

If this looks good, let me know and I can submit another PR to update documentation.