Fixed costmap cost calculation issue