Initial commit for catkinized version