Added simple automode interface
sdu_kjen_sandbox: Working on waypoint navigation
pose_2d_estimator: Minor source profiling