CostMap and LaserScanDataSource implemented