Use thread for contact monitor computation and add option to publish contact markers

[Levi-Armstrong]

No description provided.

[Levi-Armstrong]

This updates #74 to the latest version and adds option to plot contact markers to visualize in rviz.

[Levi-Armstrong]

@johnwason Do you mind taking a look to make sure I correctly fixed conflicts?

[mpowelson]

I started to make some comments. Some of them are below. This is probably fine the way it is. However, a couple of somewhat related thoughts

1. It seems like we might want to put all of this into a class to get rid of the global variables. Having multiple contact monitors running is a thing I think we might want to have. An example would be where one has a subset of links or we have one that is publishing a different contact distance
2. We might want to separate the various processes more completely. For example, I would think contactTest should go in a different thread from visualization. Then that could be running for example on the GPU. Along those lines, computeCollisionReportThread currently blocks callbackJointState. I would think the joint state callback should be really quick and then when the collision checking needs them it would very quickly lock, copy the latest, and unlock the joint states.

[johnwason]

I agree that putting the logic into a class and breaking up functionality to different threads is a good idea. This work is outside the scope of my current project so unfortunately I can't spend time on these improvements.

Is GPU collision detection available now?

[mpowelson]

Is GPU collision detection available now?

Not yet, but I know it is on the todo list(or at least the to look into list).

[Levi-Armstrong]

@mpowelson Lets create an issues with your suggested improvements and mark it as an enhancement so it does not get forgotten.