Check what happens during the switch
Name remapping of controllers should be implemented similar to ROS remapping ?
Add the ability to update heading in fully actuated without full recalculation