Minor changes

- More descriptive comments useful when doing parallel plan
- Feature to allow color console output (stolen straight from MoveIt!)
It seems strange that esc-sequences for ANSI colors are not defined in some header file, but I guess this is easier. Where are the colors being used? Is that a separate pull request?