Look into ROS actionlib to replace this in a more generic resuable way ?
Add controller registration and checking that neccessary controllers run.
Consider making everything async ?
Extract and generalize the path generation (Bezier curves from NURC)
Add support for external surge selection
Add support for external radius selection
Use doxygen grouping and reduce size
Document difference between auto-windup and ext-windup
Rename lastError, lastState to yk_1, ek_1,ek_2 etc.