Add rostest for catkin

this PR take over #21  and #6