Clean Code and Documentation
   * Clean code
   * Documentation of code
   * Add parameters to launch file