An extended infinitam version with a ROS node - View it on GitHub