restructure and optimize gazebo controllers