Add rostest based automated tests

I need to test the ROS services and topics, currently they have no coverage.