Compiling and almost working right, just get's caught up somewhere with the ROS libraries