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 !!
From next time please try not to mix formatting change and the feature addition.