Minor changes

- More descriptive comments useful when doing parallel plan
- Feature to allow color console output (stolen straight from MoveIt!)
Hmm. Does this work on windows / mac?