added code documentation and made code ROS style guide compliant