added some TODO comments and logging
replaced most of the previous code since I am integrating with ROS
adding more skeleton code