cleaned up, and changed prints to rospy outputs