fixed costmap issues. the test door detector works as expected