Its interesting that you bring this up, because we have a similar degree of complexity, both in terms of code structure, features as well as physical lines of code. The abstraction building you describe is something we also did up front, building a neat little framework that supports and organizes the abstractions(with similar queueing systems etc). We also modularized the logic-level control over entire mechanisms, and have all these moving pieces setup in a threaded environment.
Something interesting we did was to implement an event-driven system that allows for asynchronous control, akin to AWT Events(MouseListener, KeyListener, ActionListener). This makes it so that many things that happen in teleoperated and autonomous modes arent run continuously, instead triggered by a button event or a sensor event or something.
If you are curious, we've published the event model and the entire framework code at
http://code.google.com/p/grtframework for teams to peruse and use.