Manual merge of catkinized version
Added files missed in last commit