workig costmap and pathfinding functions