Add rostest for catkin

this PR take over #26 and #6