Updated to Catkin and tweaked the code