add more costmap_tools tests