now using a copy of the costmap in all handler functions without messing with the original costmap. fixed a typo in determining location