[ros-diffs] [cwittich] 16: Move over code which I have written from scratch