Add debug print function to CostMap.