Minor changes

- More descriptive comments useful when doing parallel plan
- Feature to allow color console output (stolen straight from MoveIt!)
I am fine merging this, but I'd move these color definitions in the .cpp file defining OutputHandlerSTD. There we can then use them for error msgs / warnings, etc.