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 !!