Initial cleanup through core_object_server. Clarified debugging messages. Changed printfs to ROS_INFOs. Cleaned up some code by adding references and consts. Changed some variable names to be more indicative of their function.