Ported AutonomousCommandGroup to cpp