Refactor.
Add costmap class.
Use costmap importer.
Initialize decision tree with costmap.