cleaned up code
added some new params
cleaned up source code, replaced printf with ROS_INFO, ...