Add debug print function to CostMap.
refs #13 Add placeholder functions.