As for writing code for Gazebo, check out this post: How to code for gazebo simulation? - #2 by bshah