Add graph_util to separate out some commonly used functions for drawing to the graphics object. Updated cont_a_star and vfh_a_star accordingly.