Changed std::cout for ROS_DEBUG. This fixes #3