Additional functionality to make mpc_controller_node more robust and generally usable: - General cleanup (organization, build scripts, noexcept, visiblity control) - Handle trajectories with sample periods not exactly as expected - Handle heading singularity (i.e. going from -PI to PI) - Error handling in node - Diagnostics in node