have to add convert codes(OGMap to COccupancyGridMap2D,std::deque<TPoint2D> to Path2D)