Since I don't think a true optimal route is necessary here I would have checked out RRT instead of a*. There are variants of it that are much much faster that c

Since I don't think a true optimal route is necessary here I would have checked out RRT instead of a*. There are variants of it that are much much faster that can find almost optimal routes. If you guys want to explore this further you could look into integrating ompl[1]. It has a variety of planners that could be used. I believe you could get an order of magnitude improvement using something like that. [1] https://github.com/ompl/ompl https://github.com/ompl/ompl