Added README, ROS Diagram, comments to cpp files