added lots of ROS_DEBUG statements