Utilities and basic ROS nodes wrapping MRPT functionality