Minor changes

- More descriptive comments useful when doing parallel plan
- Feature to allow color console output (stolen straight from MoveIt!)