remove costmap copying
use ros msg for Frontier structure