Improve project directory structure. Add gazebo plugins