Action Server PR Proposal

I just wanted to show you my work before i forget about it... I added a lot of really good error checking, fixed some bugs of yours, etc. Though I never got it fully working in Gazebo so I guess I can't say its better than yours. Its slick though.