Feature/catkin

Catkin + upstream