Some of the problems in 'mapping' were caused due to time inconsistencies between scans and transforms; I have a temporary fix for that, which unfortunately couples 'mapping' with 'robot', so I suppose I will have to find a better solution to the timing problem