nostart of implementing create max edges. also removed the need to run "updateCostmapMatrix" functions and some code cleaning