addign more debug statements
add enable/disable switch for groups for easy behaviour switches
daryl back in visualization, reworking teleop now with waypoint planners