Highlight selected links in publishRobotState() + improved collision visualization

[henningkayser]

The current implementation of publishRobotState() allows coloring all links with a specific color. This PR adds an optional argument highlight_links for specifying only a set of link names that should be highlighted with the given color. One example scenario would be to highligh links if they are in collision.

I'm also thinking of adding another function visualizeCollisionResult() or even checkCollision() that runs a collision check and then highlights the colliding links and publishes contact points using publishContactPoints().

UPDATE - I extended this PR

• adds checkAndPublishCollision() for performing collision checks and highlighting the results
• adds publishContactPoints() that allows passing CollisionResult::ContactMap which allows visualizing contact points for already performed collision checks

[henningkayser]

@davetcoleman I extended this PR with improved collision visualization functions since the code is closely related.

[davetcoleman]

Travis is failing on master, unrelated to this PR. I've attempted to fix here: https://github.com/ros-planning/moveit_visual_tools/pull/47

Merging this now so we can continue to contribute to open source on a client project.

[davetcoleman]

Cherry picked to master.