Mapping chages to support holonomic odometry readings..
Tests on rosbags work fine - but before this is merged into the master it has to be tested on the real robots..
