Add tf2 functionality

[mstoelzle]

I think it could be useful, to in addition to our custom RigidBodyArray message, also publish tf2 transform messages (with the tf2_ros::TransformBroadcaster, which are better supported by the ROS ecosystem. Could you please look into this wether this is useful and easy to implement?

https://docs.ros2.org/foxy/api/tf2_ros/

The idea would be that we still publish the RigidBodyArray, which contain more Mo-Cap-specific information, but also publish these standard tf2 messages which can be easily read by for example rviz. We would publish these tf2 messages for all rigid bodies, both in inthe MoCap client (e.g. in the world frame) and in the w2b package for the base frame of the robot.