Implement collision checker node

https://github.com/start-jsk/rtmros_hironx/issues/36 Implement new node for stop motion when collisions are detected.

Do not merge yet !!
Oh, It's copy/pasted from the other program. That is set right here.