Minor changes

- More descriptive comments useful when doing parallel plan
- Feature to allow color console output (stolen straight from MoveIt!)
Agreed. As we had already discussed. Colors should be used consistently. Also, they should only be used if isatty(...) returns true.