Implemented smooth planning

 - Add speed parameter in launch file
 - Refactored code into smaller functions